"""
基于有限状态机的白方防御策略
实现分层状态机：全局策略状态 + 单位个体状态

作者：何逸凡
版本：2.1 - FSM策略实现

函数组织结构：
1. 全局状态管理函数
2. DEPLOYMENT阶段函数
3. PATROL阶段函数  
4. DEFENSIVE阶段函数
5. 通用工具函数
6. 无人机相关函数
"""


import numpy as np
import math
import sys
sys.path.insert(0, sys.path[0]+"/../")


from typing import List, Optional, Tuple, Dict
from enum import Enum, auto
import scipy.optimize as opt

from agents.base_agent import RuleBasedAgent, AgentAction, AgentObservation
from algorithm.dynamic_uav_planner import group_points_by_y_kmeans, optimal_group_w_assignment
from core.units import WhiteBoat, WhiteDrone, BlackBoat
from core.config import GameConfig
from algorithm.tsp_solver import solve_tsp_in_memory  # 新增：导入TSP内存解算器
 

class GlobalState(Enum):
    """全局策略状态"""
    DEPLOYMENT = auto()      # 部署阶段：建立防线
    PATROL = auto()         # 巡航阶段：前往拦截
    DEFENSIVE = auto()      # 防守阶段：敌人接近防线


class TaskAssignmentManager:
    """任务分配管理器 - 负责白方无人船的目标分配"""
    
    def __init__(self):
        """初始化任务分配管理器"""
        self.last_assignment = None
    
    
    def is_ready(self) -> bool:
        """检查任务分配器是否准备就绪"""
        return True
    
    def assign_targets_to_boats(self, targets: List[Dict], boats) -> Dict[int, List[Dict]]:
        """
        为白方无人船分配目标
        
        Args:
            targets: 目标信息列表 [{'id': tid, 'position': (x, y), ...}, ...]
            boats: 白方无人船列表（任何具有id, x, y属性的对象）
            
        Returns:
            Dict[boat_id, assigned_targets]: 分配结果
        """
        if not self.is_ready():
            print("[TaskAssignment] 任务分配器未准备就绪")
            return {}
        
        if not targets or not boats:
            print("[TaskAssignment] 目标或无人船列表为空")
            return {}
        
        try:
            # 提取目标位置用于TSP计算
            target_positions = [target['position'] for target in targets]
            black_points_np = np.array(target_positions)
            w_points = [[boat.x, boat.y] for boat in boats]
            w_points_np = np.array(w_points)
            boat_ids = [boat.id for boat in boats]
            
            print(f"[TaskAssignment] 开始分配任务: {len(targets)}个目标, {len(boats)}艘无人船")
            
            # 使用y轴KMeans分组+最优配对算法
            groups = group_points_by_y_kmeans(black_points_np, n_groups=len(boats))
            groups_sorted = [sorted(group, key=lambda p: p[0]) for group in groups]
            best_assignment, min_total_cost = optimal_group_w_assignment(groups_sorted, w_points_np)
            
            # 使用字典优化查找：创建一个从坐标元组到目标信息的映射
            # 为了处理浮点数精度问题，将坐标转换为整数元组作为键
            pos_to_target_map = {
                (int(target['position'][0]), int(target['position'][1])): target 
                for target in targets
            }

            # 构建分配结果，将位置映射回完整的目标信息
            assignment_result = {}
            if best_assignment is not None:
                for i, group_idx in enumerate(best_assignment):
                    group_positions = groups_sorted[group_idx]
                    boat_id = boat_ids[i]
                    
                    # 为每个分配的位置找回对应的目标信息
                    assigned_targets = []
                    for pos in group_positions:
                        # 使用字典进行O(1)复杂度的查找
                        pos_key = (int(pos[0]), int(pos[1]))
                        target_info = pos_to_target_map.get(pos_key)
                        if target_info:
                            assigned_targets.append(target_info)
                        else:
                            print(f"[TaskAssignment] 警告: 无法为位置 {pos_key} 找到匹配的目标信息")
                    
                    assignment_result[boat_id] = assigned_targets
            
            # 记录分配历史
            self.last_assignment = {
                'targets': targets,
                'boats': boat_ids,
                'result': assignment_result,
                'cost': min_total_cost,
                'timestamp': None  # 将在调用处设置
            }
            
            print(f"[TaskAssignment] 任务分配完成，总代价: {min_total_cost:.2f}")
            self._log_assignment_details(assignment_result, w_points, boat_ids)
            
            return assignment_result
            
        except Exception as e:
            print(f"[TaskAssignment] 任务分配失败: {e}")
            return {}
    


    def _log_assignment_details(self, assignment: Dict[int, List], w_points: List, boat_ids: List):
        """记录分配详情"""
        for i, boat_id in enumerate(boat_ids):
            target_infos = assignment.get(boat_id, [])
            if not target_infos:
                print(f"  舰船{boat_id}（起点: {w_points[i]}）未分配目标点")
            else:
                print(f"  舰船{boat_id}（起点: {w_points[i]}）分配{len(target_infos)}个目标:")
                for j, target_info in enumerate(target_infos):
                    target_id = target_info['id']
                    pos = target_info['position']
                    print(f"    目标{j+1}: ID={target_id}, 位置({pos[0]:.0f}, {pos[1]:.0f})")
    
    def get_assignment_statistics(self) -> Dict:
        """获取分配统计信息"""
        if not self.last_assignment:
            return {}
        
        return {
            'target_count': len(self.last_assignment['targets']),
            'boat_count': len(self.last_assignment['boats']),
            'total_cost': self.last_assignment['cost'],
            'avg_targets_per_boat': len(self.last_assignment['targets']) / len(self.last_assignment['boats'])
        }


class FSMDefenseAgent(RuleBasedAgent):
    """
    基于有限状态机的白方防御策略
    
    采用全局状态机设计：
    - 全局状态机：控制整体策略和资源分配
    - 直接基于全局状态决策，无需复杂的个体状态管理
    """
    
    def __init__(self):
        super().__init__("FSMDefenseAgent", "defensive")
        
        # 全局状态
        self.global_state = GlobalState.DEPLOYMENT
        self.state_timer = 0.0
        
        # 状态数据
        self.boat_targets = {}    # {boat_id: target_position}
        self.drone_targets = {}   # {drone_id: target_info}
        self.detected_targets = {}  # {target_id: enhanced_target_data}
        
        # 任务分配管理器
        self.task_manager = TaskAssignmentManager()
        
        # DEPLOYMENT 阶段参数
        self.deployment_line_x = 60000
        self.deployment_line_y_start = 61655      # 线段起始y坐标
        self.deployment_line_y_end =611550      # 线段结束y坐标
        
        # 封锁线坐标（A1A6线）
        self.blockade_line_x = 50000

        # 无人机任务状态
        self.drone_mission_state = {}  # {drone_id: mission_info}
        self.drone_waypoints = self._init_drone_waypoints()
        
        # 黑方目标分组 (TODO: 待实现)
        self.target_groups = {}  # {group_id: [target_ids]}
        self.boat_group_assignments = {}  # {boat_id: group_id}
        
        # 新增：记录每艘无人船的拦截信息
        self.boat_intercept_info = {}  # {boat_id: {'target_id': int, 'intercept_point': (x, y), 'intercept_time': float, 'scan_index': int}}
        
        # 拦截点缓存机制 - 避免在目标未探测到时拦截点变化
        self.cached_intercept_points = {}  # {boat_id: {'target_id': str, 'intercept_point': (x, y), 'last_update': float}}
        
        # DEFENSIVE阶段无人机警戒扫视状态
        self.drone_alert_state = {}  # {drone_id: {'state': 'charging'|'alert_scanning'|'monitoring', 'last_alert_time': float, 'target_id': str|None, 'scan_center': (x,y), 'scan_angle': float}}
        
        # 新增：无人机发现的近距离目标通知
        self.drone_close_target_alerts = {}  # {boat_id: {'target_id': str, 'alert_time': float, 'handled': bool}}
        
        # 新增：目标击沉后的通知机制
        self.target_destruction_notifications = []  # [(target_id, original_assigned_boat_id, destruction_time), ...]
        
        # 调试计数器
        # 调试计数器
        self._target_update_debug_count = 0

        # 2号无人机TSP路径相关
        self._drone2_tsp_path = None
        self._drone2_tsp_index = 0


    def _init_drone_waypoints(self) -> Dict[int, List[Tuple[float, float]]]:
        """初始化无人机航点（与simple_line_defense_agent相同）"""
        return {
            0: [(244000, -280000+336302), (264000, -208000+336302)],  # 0号无人机（最下面）
            1: [(265000, -48000+336302), (265000, -139000+336302)],   # 1号无人机
            2: [(244000, 336302), (264000, 336302)],        # 2号无人机
            3: [(265000, 48000+336302), (265000, 139000+336302)], # 3号无人机
            4: [(244000, 280000+336302), (264000, 208000+336302)] # 4号无人机（最上面）
        }

    def reset(self):
        """重置FSM状态"""
        super().reset()
        self.global_state = GlobalState.DEPLOYMENT
        self.state_timer = 0.0
        self.boat_targets.clear()
        self.drone_targets.clear()
        self.detected_targets.clear()
        self.drone_mission_state.clear()
        self.target_groups.clear()
        self.boat_group_assignments.clear()
        self.cached_intercept_points.clear()
        self.drone_alert_state.clear()
        self.drone_close_target_alerts.clear()
        self.target_destruction_notifications.clear()

        # 重置2号无人机TSP路径
        self._drone2_tsp_path = None
        self._drone2_tsp_index = 0


        # 重置任务分配状态
        if hasattr(self, '_final_assignment_done'):
            delattr(self, '_final_assignment_done')
        
        # 重新初始化任务分配管理器
        self.task_manager = TaskAssignmentManager()
    
    def get_actions(self, observation: AgentObservation) -> AgentAction:
        """主要决策函数"""
        current_time = observation.current_time
        
        # 更新状态计时器
        if hasattr(self, '_last_time'):
            self.state_timer += current_time - self._last_time
        self._last_time = current_time
        
        # 更新目标信息
        self._update_detected_targets(observation)
        # print(observation.get_detected_targets())
        # print(self.detected_targets)
        # 全局状态机更新
        self._update_global_state(observation)
        

        
        # 生成动作
        action = AgentAction()
        
        # 无人艇动作
        active_boats = observation.get_active_boats()
        boat_actions = np.zeros((5, 3))
        for boat in active_boats:
            boat_actions[boat.id] = self._generate_boat_action(boat, observation)
        action.boat_actions = boat_actions
        
        # 无人机动作
        # 获取所有无人机（包括停靠在母舰上的无人机）
        all_drones = observation.all_drones
        if all_drones:
            drone_actions = np.zeros((len(all_drones), 3))
            for drone in all_drones:
                drone_actions[drone.id] = self._generate_drone_action(drone, observation)
            action.drone_actions = drone_actions

        
        # 验证任务分配状态
        # if self.global_state == GlobalState.PATROL and not self.validate_task_assignment():
        #     print("[FSM] 警告：PATROL状态下任务分配无效，尝试重新分配")
        #     self.force_reassign_tasks(observation)


        
        return action

    # ==================== 任务分配函数 ====================
    
    def _assign_tasks(self, observation: AgentObservation):
        """
        进行任务分配
        """
        
        print(f"[FSM] 执行任务分配，时间: {observation.current_time}")
        # 收集可拦截的目标
        interceptable_targets = self._collect_interceptable_targets(observation)
        if not interceptable_targets:
            print("[FSM] 没有可拦截的目标，无法进行任务分配")
            return
        # 获取白方无人船
        white_boats = [boat for boat in observation.white_boats if boat.active]
        if not white_boats:
            print("[FSM] 没有可用的白方无人船")
            return
        # 执行任务分配
        assignment_result = self.task_manager.assign_targets_to_boats(interceptable_targets, white_boats)
        
        if assignment_result:
            # 应用分配结果
            self._apply_task_assignment(assignment_result, observation.current_time)
            
            
            
    

    
    # ==================== 目标管理函数 ====================
    
    def _update_detected_targets(self, observation: AgentObservation):
        """更新目标信息，使用统一简洁的数据结构"""
        now = observation.current_time
        # 首先更新所有历史目标的预测位置（包括当前未观测到的）
        for target_id, target_data in self.detected_targets.items():
            if target_data['is_frozen'] :
                # 如果目标被冻结，检查冻结时间是否已过
                if  now >= target_data['frozen_until']:
                    # 解冻目标
                    target_data['is_frozen'] = False
                    target_data['frozen_until'] = None
                    print(f"[FSM] 目标{target_id}已解冻")
                else:
                    target_data['speed']= 0.0  # 冻结状态下速度为0
                    target_data['predict_pos'] = target_data['position']  # 保持当前位置不变
            time_diff = now - target_data['last_seen']
            
            # 基于最后观测到的位置、速度、航向预测当前位置
            pred_x = target_data['position'][0] + target_data['speed'] * math.cos(target_data['heading']) * time_diff
            pred_y = target_data['position'][1] + target_data['speed'] * math.sin(target_data['heading']) * time_diff
            target_data['predict_pos'] = (pred_x, pred_y)
        
        # 然后更新当前观测到的目标
        detected_targets = observation.get_detected_targets()
        #print(f"[DEBUG] 观测到的目标数量: {len(detected_targets)}")
        
        for target in detected_targets: 
            target_pos = (target.x, target.y)
            # print(f"[DEBUG] 正在更新目标 {target.id} 到 detected_targets")
            
            # 使用用户要求的数据结构
            self.detected_targets[target.id] = {
                'id': target.id,
                'position': target_pos,
                'speed': target.speed,
                'heading': target.heading,
                'last_seen': now,
                'predict_pos': target_pos,  # 当前观测到的目标，预测位置就是当前位置
                'is_frozen': False,  # 是否被冻结
                'frozen_until': None  # 冻结到的时间
            }
            
       #print(f"[DEBUG] 更新后 detected_targets 包含 {len(self.detected_targets)} 个目标")
            

    def _collect_interceptable_targets(self, observation: AgentObservation) -> List[Dict]:
        """收集所有可拦截的目标信息"""
        interceptable_targets = []
        white_boats = [boat for boat in observation.white_boats if boat.active]
        
        total_targets = len(self.detected_targets)
        filtered_count = 0
        
        for target_id, target_data in self.detected_targets.items():
            
            # 检查是否可以拦截
            if self._can_target_be_intercepted(target_data, white_boats):
                interceptable_targets.append({
                    'id': target_id,
                    'position': target_data['position'],
                    'speed': target_data['speed'],
                    'heading': target_data['heading']
                    
                })
            else:
                filtered_count += 1
                # 添加过滤日志
                print(f"[FSM] 目标{target_id}在位置{target_data['predict_pos']}不可拦截，已从任务分配中过滤")
        
        print(f"[FSM] 拦截目标过滤完成：总目标{total_targets}个，可拦截{len(interceptable_targets)}个，过滤{filtered_count}个")
        return interceptable_targets

    
    def _calculate_intercept_point_with_id(self, boat: WhiteBoat, target_id: str, observation: AgentObservation):

        """根据目标ID计算拦截点"""
        target_data = self.detected_targets.get(target_id)
        if not target_data:
            return None
            
        intercept_result = self._calculate_intercept_point(
            boat, target_data['position'], target_data['speed'], target_data['heading']
        )
        if intercept_result is None:
            return None
        intercept_pose, intercept_time, boat_pos = intercept_result
        return intercept_pose

    def _can_target_be_intercepted(self, target_info: Dict, white_boats: List[WhiteBoat]) -> bool:
        """检查目标是否可以被拦截"""
        for boat in white_boats:
            intercept_result = self._calculate_intercept_point(
                boat, target_info['predict_pos'], 
                target_info.get('speed', 0.0), 
                target_info.get('heading', 0.0)
            )
            if intercept_result is not None:
                return True
        return False
    
    def _apply_task_assignment(self, assignment: Dict[int, List[Dict]], timestamp: float):
        """应用任务分配结果"""
        # 更新boat_targets，保存目标ID而不是位置
        for boat_id, target_infos in assignment.items():
            self.boat_targets[boat_id] = [target['id'] for target in target_infos]
        

        
        # 更新任务分配管理器的时间戳
        if self.task_manager.last_assignment:
            self.task_manager.last_assignment['timestamp'] = timestamp
        
        print(f"[FSM] 任务分配结果已应用到boat_targets（保存目标ID）")
        # 打印分配详情
        for boat_id, target_ids in self.boat_targets.items():
            if target_ids:
                print(f"  船只{boat_id}分配到目标ID: {target_ids}")
    

    
    def force_reassign_tasks(self, observation: AgentObservation) -> bool:
        """强制重新分配任务（用于动态调整）"""
        print("[FSM] 强制重新分配任务")
        
        # 重置分配状态
        if hasattr(self, '_final_assignment_done'):
            delattr(self, '_final_assignment_done')
        
        # 执行分配
        self._assign_tasks(observation)
        
        return hasattr(self, '_final_assignment_done') 
    
    def validate_task_assignment(self) -> bool:
        """验证当前任务分配的有效性"""
        if not self.boat_targets:
            return False
        
        # 检查是否有船只分配了目标
        has_assigned_targets = any(len(targets) > 0 for targets in self.boat_targets.values())
        
        # 检查任务分配管理器状态
        manager_valid = self.task_manager.is_ready() and self.task_manager.last_assignment is not None
        
        return has_assigned_targets and manager_valid
    

    # 全局状态机更新
    # ==================== 全局状态管理函数 ====================
    
    def _update_global_state(self, observation: AgentObservation):
        """更新全局状态机"""
        current_state = self.global_state

        if current_state == GlobalState.DEPLOYMENT:
            # 部署完成条件：所有无人机完成航点探测任务
            if self._should_enter_PATROL_phase(observation):
                #记录第一阶段时间
                print(f"[FSM] 部署阶段完成，时间: {observation.current_time}")
                print("===进入 PATROL 阶段===")
                self.global_state = GlobalState.PATROL
                self.state_timer = 0.0
                # 执行任务分配
                self._create_drone_patrol_path(observation)
                self._assign_tasks(observation)
                
        elif current_state == GlobalState.PATROL:
            # 检查是否应该进入DEFENSIVE阶段
            if self._should_enter_defensive_phase(observation):
                self.global_state = GlobalState.DEFENSIVE
                self.state_timer = 0.0
                print(f"[FSM] 进入DEFENSIVE阶段，时间: {observation.current_time}")

        elif current_state == GlobalState.DEFENSIVE:
            # DEFENSIVE阶段：敌人接近防线
            # 无人机跟踪和搜索，无人船锁定一切可锁定的目标
            pass
        
    #状态判断
    def _should_enter_PATROL_phase(self, observation: AgentObservation) -> bool:
        """
        检查部署是否完成
        完成条件：所有无人机完成航点探测任务（到达所有航点并完成圆形扫描后返回母舰）
        """
        # 检查每架无人机的任务状态
        for drone_id in [0,1,2,3,4]:  # 5架无人机
            mission_state = self.drone_mission_state.get(drone_id, {})
            
            # 如果无人机的任务还没有完成
            if mission_state.get('phase') != 'mission_completed':
                return False
        
        return True
    
    def _should_enter_defensive_phase(self, observation: AgentObservation) -> bool:
        """
        检查是否应该进入DEFENSIVE阶段
        进入条件：任何黑方无人船的预测位置进入白方无人船45km范围内
        """
        # 条件：检查是否有黑方无人船预测位置在白方无人船45km范围内
        black_boats_in_range = self._check_black_boats_in_45km_range(observation)
        
        if black_boats_in_range:
            print(f"[FSM] DEFENSIVE条件满足：有黑方无人船可能出现在在白方无人船45km范围内")

        return black_boats_in_range

    def _check_black_boats_in_specific_white_45km_range(self, observation: AgentObservation, boat: WhiteBoat) -> List[int]:
        """检查是否有黑方无人船预测位置在指定白方无人船45km范围内
        返回符合条件的黑方无人船ID列表"""
        
        threat_id = []
        for target_id, target_data in self.detected_targets.items():
            pos = target_data['position']
            last_seen = target_data['last_seen']
            now = observation.current_time
            # 检查黑船是否可能在任一白方无人船45km范围内
            
            distance = math.hypot(pos[0] - boat.x, pos[1] - boat.y)-(now - last_seen)*GameConfig.BLACK_BOAT_MAX_SPEED
            if distance <= 45000:
                threat_id.append(target_id)
        return threat_id
    def _check_black_boats_in_45km_range(self, observation: AgentObservation) -> bool:
        """检查是否有黑方无人船预测位置在任一白方无人船45km范围内"""
        white_boats = [boat for boat in observation.white_boats if boat.active]
        
        for target_id, target_data in self.detected_targets.items():
            pos = target_data['position']
            last_seen = target_data['last_seen']
            now = observation.current_time
            # 检查黑船是否可能在任一白方无人船45km范围内
            for boat in white_boats:
                distance = math.hypot(pos[0] - boat.x, pos[1] - boat.y)-(now - last_seen)*GameConfig.BLACK_BOAT_MAX_SPEED
                if distance <= 45000:
                    return True
        
        return False
    
    
    def _generate_boat_action(self, boat: WhiteBoat, observation: AgentObservation) -> np.ndarray:
        """根据全局状态生成单艘无人艇动作"""
        # 检查是否被冻结
        if boat.is_frozen(observation.current_time):
            return np.zeros(4)
        
        # 根据全局状态决策
        if self.global_state == GlobalState.DEPLOYMENT:
            return self._execute_deployment_boat_action(boat, observation)
        elif self.global_state == GlobalState.PATROL:
            return self._execute_patrol_boat_action(boat, observation)
        elif self.global_state == GlobalState.DEFENSIVE:
            return self._execute_defensive_boat_action(boat, observation)
        else:
            return np.zeros(4)

    # ==================== PATROL阶段函数 ====================
    
    def _execute_patrol_boat_action(self, boat: WhiteBoat, observation: AgentObservation) -> np.ndarray:
        """执行PATROL阶段的无人船行为"""
        # 获取分配的目标ID列表
        assigned_target_ids = self.boat_targets.get(boat.id)
        
        if not assigned_target_ids:
            print(f"[FSM] Boat {boat.id} 在PATROL阶段未分配目标，保持位置")# todo 如果patrol阶段没有分配目标,应该有支援的操作
            return self._boat_hold_position(boat, observation)
        
        # 选择第一个分配的目标进行拦截
        target_id = assigned_target_ids[0]
        
        # 计算拦截点
        intercept_point = self._calculate_intercept_point_with_id(boat, target_id, observation)
        
        if intercept_point is not None:
            intercept_x, intercept_y = intercept_point
            dx = intercept_x - boat.x
            dy = intercept_y - boat.y
            distance = math.hypot(dx, dy)
            heading = math.atan2(dy, dx)
            speed =boat.max_speed
            
        else:
            # 如果无法计算拦截点，直接选择下一个目标
            speed = 0.0
            heading = 0.0
            # self.boat_SOS(boat, observation)
            assigned_target_ids.pop(0)
            print(f"[FSM] PATROL阶段:Boat {boat.id} 无法计算目标{target_id}的拦截点，计算下一个目标")

        # PATROL阶段使用lock_id = target_id表示锁定第一个目标
        return np.array([speed, heading, target_id])
    
    def _execute_patrol_drone_action(self, drone: WhiteDrone, observation: AgentObservation) -> np.ndarray:
        """执行PATROL阶段的探测无人机行为"""
        
        if drone.id == 2:
            
            # 确保TSP路径已生成
            if not hasattr(self, '_drone2_tsp_path') or self._drone2_tsp_path is None:
                print(f"[FSM] 2号无人机TSP路径未生成，重新生成")
                self._create_drone_patrol_path(observation)
                if not hasattr(self, '_drone2_tsp_index'):
                    self._drone2_tsp_index = 0
            
            # 动态移除已探测目标
            detected_ids = set(t.id for t in observation.get_single_detected_targets(drone))
            #print(f"[FSM] 2号无人机探测到目标ID: {detected_ids}")
            

            # 只保留未被探测到的目标id（使用unit_id进行精确匹配）
            old_path = self._drone2_tsp_path if self._drone2_tsp_path is not None else []
            self._drone2_tsp_path = [tid for tid in old_path if tid not in detected_ids] if old_path is not None else []

            # 如果路径发生变化，打印调试信息
            if len(old_path) != len(self._drone2_tsp_path):
                removed_ids = set(old_path) - set(self._drone2_tsp_path)
                print(f"[FSM] 2号无人机从TSP路径中移除目标: {removed_ids}")
                print(f"[FSM] 2号无人机TSP路径中的目标ID: {self._drone2_tsp_path}")
                # 如果当前目标被移除，自动跳到下一个
                if self._drone2_tsp_path==[] or removed_ids == self._drone2_tsp_path[0]:
                    self._drone2_tsp_index += 1
                    print(f"[FSM] 2号无人机跳到下一个目标: {self._drone2_tsp_index}")
              
            # 路径为空或已完成
            if self._drone2_tsp_index >= len(self._drone2_tsp_path):
                return self._drone_return_to_parent_action(drone, observation)
            
            target_id = self._drone2_tsp_path[self._drone2_tsp_index]
            # 获取目标信息
            target_data = self.detected_targets.get(target_id)
            if not target_data:
                # 目标丢失，跳到下一个
                self._drone2_tsp_index += 1
                print(f"[FSM] 2号无人机目标{target_id}丢失，跳到下一个")
                return np.array([0.0, drone.heading, 1.0])
                
            pred_x, pred_y = target_data['predict_pos']
            
            # 检查预测点是否在封锁线后
            if pred_x < self.blockade_line_x:
                print(f"[FSM] 2号无人机目标 {target_id} 预测点在封锁线后，跳过此目标")
                x, y = target_data['position']
                print(f"[FSM] 目标当前位置: ({x:.0f}, {y:.0f}), 预测位置: ({pred_x:.0f}, {pred_y:.0f})")
                self._drone2_tsp_index += 1
                return np.array([0.0, drone.heading, 1.0])
            
            dx = pred_x - drone.x
            dy = pred_y - drone.y
            dist = math.hypot(dx, dy)
            if dist < 500:  # 到达目标
                print(f"[FSM] 2号无人机到达目标 {target_id}，距离: {dist:.0f}m，当前位置: ({drone.x:.0f}, {drone.y:.0f})")
                self._drone2_tsp_index += 1
                # 检查是否还有下一个目标
                if self._drone2_tsp_index >= len(self._drone2_tsp_path):
                    print(f"[FSM] 2号无人机完成所有目标访问，开始返航")
                    return self._drone_return_to_parent_action(drone, observation)
                return np.array([0.0, drone.heading, 1.0])
            nav_heading = math.atan2(dy, dx)
            nav_speed = drone.max_speed
            #print(f"[FSM] 2号无人机前往目标 {target_id}，距离: {dist:.0f}m，速度: {nav_speed:.1f}m/s")
            return np.array([nav_speed, nav_heading, 1.0])

        return np.array([0.0, 0.0, 1.0])

    # ==================== DEFENSIVE阶段函数 ====================
    
    def _execute_defensive_boat_action(self, boat: WhiteBoat, observation: AgentObservation) -> np.ndarray:
        """执行DEFENSIVE阶段的无人船行为 - 重构版本"""  
        # 清理无效目标
        self._cleanup_invalid_targets(boat, observation)
        
        # 统一的目标选择和行为执行
        target_id, action_type = self._find_best_target(boat, observation)
        
        if target_id:
            return self._engage_target(boat, target_id, action_type, observation)
        else:
            return self._maintain_defensive_position(boat, observation)
    
    def _find_best_target(self, boat: WhiteBoat, observation: AgentObservation) -> Tuple[Optional[str], str]:
        """统一的目标选择逻辑，返回(target_id, action_type)"""
        print(f"[FSM] DEFENSIVE: Boat {boat.id} 开始寻找新目标...")
        
        # 优先级1: 处理当前锁定目标
        if boat.current_target and boat.current_target.active:
            print(f"[FSM] DEFENSIVE: Boat {boat.id} 优先级1: 继续锁定当前目标 {boat.current_target.id}")
            return str(boat.current_target.id), "current_lock"
        elif boat.current_target:
            print(f"[FSM] DEFENSIVE: Boat {boat.id} 当前目标 {boat.current_target.id} 已失效")
        
        # 优先级2: 处理无人机发现的近距离目标（新增）
        if boat.id in self.drone_close_target_alerts:
            alert = self.drone_close_target_alerts[boat.id]
            if not alert['handled']:
                target_id = alert['target_id']
                target = self._find_target_by_id(target_id, observation)
                if target and target.active:
                    # 验证目标确实在40km内
                    distance = boat.distance_to(target)
                    if distance <= 40000:
                        print(f"[FSM] DEFENSIVE: 船只 {boat.id} 优先级2: 处理无人机发现的近距离目标 {target_id}")
                        # 将目标临时添加到船只的目标列表最前面（如果不在列表中）
                        boat_targets = self.boat_targets.get(boat.id, [])
                        if target_id not in boat_targets:
                            boat_targets.insert(0, target_id)
                            self.boat_targets[boat.id] = boat_targets
                            print(f"[FSM] DEFENSIVE: 将近距离目标 {target_id} 临时添加到船只 {boat.id} 目标列表")
                        return target_id, "drone_guided_close"
                
                # 目标无效，标记警报为已处理
                alert['handled'] = True
        
        # 优先级3: 处理45km探测范围内的威胁目标
        threats = self._check_black_boats_in_specific_white_45km_range(observation, boat)
        if threats:
            nearest_threat_id = self._find_nearest_threat(boat, [str(t) for t in threats])
            if nearest_threat_id:
                print(f"[FSM] DEFENSIVE: Boat {boat.id} 优先级3: 处理威胁目标 {nearest_threat_id}")
                return nearest_threat_id, "immediate_threat"
        
        # 优先级4: 处理分配的目标
        assigned_targets = self.boat_targets.get(boat.id, [])
        if assigned_targets:
            print(f"[FSM] DEFENSIVE: Boat {boat.id} 优先级4: 处理分配的目标 {assigned_targets[0]}")
            return str(assigned_targets[0]), "assigned"
        else:
            print(f"[FSM] DEFENSIVE: Boat {boat.id} 没有分配的目标")
        
        # 优先级5: 处理可拦截的目标
        nearest_interceptable = self._find_nearest_interceptable_target(boat, observation)
        if nearest_interceptable:
            print(f"[FSM] DEFENSIVE: Boat {boat.id} 优先级5: 处理可拦截目标 {nearest_interceptable}")
            return nearest_interceptable, "interceptable"
        
        # 优先级6: 处理探测到的目标（新增）- 在DEFENSIVE阶段直接处理探测到的目标
        if boat.detected_targets:
            print(f"[FSM] DEFENSIVE: Boat {boat.id} 探测到 {len(boat.detected_targets)} 个目标")
            # 选择最近的在锁定范围内的目标
            lockable_targets = []
            for target in boat.detected_targets:
                distance = boat.distance_to(target)
                if distance <= GameConfig.WHITE_BOAT_LOCK_RANGE:
                    lockable_targets.append((target, distance))
            
            if lockable_targets:
                # 选择最近的目标
                nearest_target, _ = min(lockable_targets, key=lambda x: x[1])
                print(f"[FSM] DEFENSIVE: 船只 {boat.id} 优先级6: 直接锁定探测到的目标 {nearest_target.id}")
                return str(nearest_target.id), "detected"
            else:
                print(f"[FSM] DEFENSIVE: Boat {boat.id} 探测到的目标都不在锁定范围内")
        else:
            print(f"[FSM] DEFENSIVE: Boat {boat.id} 没有探测到任何目标")
        
        # 优先级7: 无目标，执行防御巡逻
        print(f"[FSM] DEFENSIVE: Boat {boat.id} 优先级7: 无目标，执行防御巡逻")
        return None, "patrol"
    
    def _engage_target(self, boat: WhiteBoat, target_id: str, action_type: str, observation: AgentObservation) -> np.ndarray:
        """统一的目标交战逻辑"""
        # 处理当前锁定目标的特殊逻辑
        if action_type == "current_lock" and boat.current_target:
            return self._handle_locking_target(boat, boat.current_target, observation)
        
        # 对于其他类型目标，先查找目标对象
        target = self._find_target_by_id(target_id, observation)
        if not target or not target.active:
            # 目标无效，清理并回到防御位置
            self._remove_invalid_target(boat, target_id)
            # 如果是无人机指引的目标失效，标记警报为已处理
            if action_type == "drone_guided_close" and boat.id in self.drone_close_target_alerts:
                self.drone_close_target_alerts[boat.id]['handled'] = True
            return self._maintain_defensive_position(boat, observation)
        
        # 处理无人机指引的近距离目标（特殊处理）
        if action_type == "drone_guided_close":
            return self._handle_drone_guided_close_target(boat, target, observation)
        
        # 根据距离决定行为策略
        distance = boat.distance_to(target)
        
        if distance <= GameConfig.WHITE_BOAT_LOCK_RANGE :
            # 在锁定范围内，开始锁定
            return self._initiate_lock(boat, target, action_type, observation)
        else:
            # 超出锁定范围，计算追击
            return self._pursue_target_unified(boat, target, action_type, observation)
    
    def _cleanup_invalid_targets(self, boat: WhiteBoat, observation: AgentObservation):
        """清理无效目标"""
        assigned_targets = self.boat_targets.get(boat.id, [])
        active_targets = [
            target_id for target_id in assigned_targets
            if any(t.id == target_id and t.active for t in observation.black_boats)
        ]
        
        # 记录被清理的目标
        removed_targets = set(assigned_targets) - set(active_targets)
        for target_id in removed_targets:
            print(f"[FSM] DEFENSIVE: 清理船只 {boat.id} 的无效目标 {target_id}")
        
        self.boat_targets[boat.id] = active_targets
    
    def _remove_invalid_target(self, boat: WhiteBoat, target_id: str):
        """移除单个无效目标"""
        assigned_targets = self.boat_targets.get(boat.id, [])
        if target_id in assigned_targets:
            assigned_targets.remove(target_id)
            self.boat_targets[boat.id] = assigned_targets
            print(f"[FSM] DEFENSIVE: 移除船只 {boat.id} 的无效目标 {target_id}")
    
    def _initiate_lock(self, boat: WhiteBoat, target: BlackBoat, action_type: str, observation: AgentObservation) -> np.ndarray:
        """开始锁定目标"""
        boat.current_target = target
        
        print(f"[FSM] DEFENSIVE: Boat {boat.id} 开始锁定目标 {target.id}，此时距离为{boat.distance_to(target):.0f}m")
        
        # 检查目标状态并执行相应动作
        if target.is_frozen(observation.current_time):
            return np.array([0.0, target.heading, target.id])
        else:
            return np.array([target.speed, target.heading, target.id])
    
    def _pursue_target_unified(self, boat: WhiteBoat, target: BlackBoat, action_type: str, observation: AgentObservation) -> np.ndarray:
        """统一的目标追击逻辑"""
        drone_action = 1.0 if action_type == "immediate_threat" else 0.0
        
        # 计算拦截点
        intercept_result = self._calculate_intercept_point(
            boat, (target.x, target.y), target.speed, target.heading
        )
        
        if intercept_result:
            intercept_point, _, _ = intercept_result
            dx = intercept_point[0] - boat.x
            dy = intercept_point[1] - boat.y
            heading = math.atan2(dy, dx)
            speed = boat.max_speed
            
            print(f"[FSM] DEFENSIVE: Boat {boat.id} 追击目标 {target.id} (类型: {action_type})")
            return np.array([speed, heading, target.id])
        else:
            print(f"[FSM] DEFENSIVE: Boat {boat.id} 无法计算目标 {target.id} 的拦截点")
            return self._maintain_defensive_position(boat, observation)
    
    def _find_nearest_threat(self, boat: WhiteBoat, threats: List[str]) -> Optional[str]:
        """寻找最近的威胁目标"""
        min_distance = float('inf')
        nearest_target_id = None
        
        for target_id in threats:
            target_data = self.detected_targets.get(target_id)
            if target_data:
                distance = math.hypot(
                    boat.x - target_data['position'][0], 
                    boat.y - target_data['position'][1]
                )
                if distance < min_distance:
                    min_distance = distance
                    nearest_target_id = target_id
        
        return nearest_target_id
    
    def _handle_drone_guided_close_target(self, boat: WhiteBoat, target: BlackBoat, observation: AgentObservation) -> np.ndarray:
        """处理无人机指引的近距离目标"""
        distance = boat.distance_to(target)
        
        # 标记警报为已处理
        if boat.id in self.drone_close_target_alerts:
            self.drone_close_target_alerts[boat.id]['handled'] = True
        
        print(f"[FSM] DEFENSIVE: 船只 {boat.id} 处理无人机指引的近距离目标 {target.id} (距离: {distance:.0f}m)")
        
        # 保存原定目标信息（如果存在）
        original_target_id = None
        boat_target_list = self.boat_targets.get(boat.id, [])
        if isinstance(boat_target_list, list) and boat_target_list:
            original_target_id = boat_target_list[0]  # 第一个目标是当前目标
        
        # 立即尝试锁定近距离目标，暂停原定任务
        if distance <= GameConfig.WHITE_BOAT_LOCK_RANGE - 100:
            # 在锁定范围内，开始锁定
            result = self._initiate_lock(boat, target, "drone_guided_close", observation)
            
            # 检查目标是否被击沉
            if not target.active:
                # 目标被击沉，恢复原定目标并重新计算拦截路线
                self._restore_original_target_after_close_engagement(boat, original_target_id, observation)
            
            return result
        else:
            # 超出锁定范围，追击
            return self._pursue_target_unified(boat, target, "drone_guided_close", observation)

    def _restore_original_target_after_close_engagement(self, boat: WhiteBoat, original_target_id: Optional[str], observation: AgentObservation):
        """在处理完近距离目标后恢复原定目标并重新计算拦截路线"""
        if original_target_id:
            # 检查原定目标是否仍然存在
            original_target = self._find_target_by_id(original_target_id, observation)
            if original_target and original_target.active:
                print(f"[FSM] DEFENSIVE: 船只 {boat.id} 处理完近距离目标，恢复原定目标 {original_target_id}")
                
                # 重新计算拦截路线
                target_data = self.detected_targets.get(original_target_id)
                if target_data:
                    intercept_result = self._calculate_intercept_point(
                        boat, target_data['position'], 
                        target_data['speed'], 
                        target_data['heading']
                    )
                    if intercept_result:
                        intercept_point, intercept_time, _ = intercept_result
                        print(f"[FSM] DEFENSIVE: 船只 {boat.id} 重新计算目标 {original_target_id} 的拦截路线")
                        print(f"                新拦截点: ({intercept_point[0]:.0f}, {intercept_point[1]:.0f})")
            else:
                # 原定目标已不存在，从目标列表中移除
                boat_target_list = self.boat_targets.get(boat.id, [])
                if isinstance(boat_target_list, list) and original_target_id in boat_target_list:
                    boat_target_list.remove(original_target_id)
                    print(f"[FSM] DEFENSIVE: 船只 {boat.id} 原定目标 {original_target_id} 已不存在，从列表中移除")
                
                # 转向下一个目标
                next_target_id = self._get_next_target_for_boat(boat)
                if next_target_id:
                    print(f"[FSM] DEFENSIVE: 船只 {boat.id} 转向下一个目标 {next_target_id}")
        else:
            print(f"[FSM] DEFENSIVE: 船只 {boat.id} 无原定目标，保持警戒位置")
    
    def _handle_locking_target(self, boat: WhiteBoat, target: BlackBoat, observation: AgentObservation) -> np.ndarray:
        """处理正在锁定的目标"""
        
        
        # 检查目标状态
        target_status = self._check_target_status(target, observation, boat)
        
        if target_status == "destroyed":
            return self._handle_target_destroyed(boat, target, observation)
        elif target_status == "frozen":
            return self._handle_target_frozen(target)
        elif target_status == "in_range":
            return self._handle_target_in_range(target)
        else:  # out_of_range
            return self._handle_target_out_of_range(boat, target, observation,)
    
    def _check_target_status(self, target: BlackBoat, observation: AgentObservation, boat: WhiteBoat) -> str:
        """检查目标状态"""
        if not target.active:
            return "destroyed"
        if target.is_frozen(observation.current_time):
            return "frozen"
        
        distance = boat.distance_to(target)
        if distance <= GameConfig.WHITE_BOAT_LOCK_RANGE:
            return "in_range"
        else:
            return "out_of_range"
    
    def _handle_target_destroyed(self, boat: WhiteBoat, target: BlackBoat, observation: AgentObservation) -> np.ndarray:
        """处理目标被击沉的情况 - 更新后的版本"""
        print(f"[FSM] DEFENSIVE: Boat {boat.id} 成功击沉目标 {target.id}")
        
        # 清理无人艇的current_target，确保它能转向新目标
        if boat.current_target and boat.current_target.id == target.id:
            boat.current_target = None
            boat.stop_locking()
            print(f"[FSM] DEFENSIVE: Boat {boat.id} 清理已击沉目标的锁定状态")
        
        # 从探测目标列表中移除
        target_id = str(target.id)
        if target_id in self.detected_targets:
            del self.detected_targets[target_id]
        
        # 从任务分配中移除
        self._remove_target_from_assignments(boat, target_id)
        
        # 记录击沉事件
        self._handle_target_destroyed_notification(target_id, boat.id, observation.current_time)
        
        # 寻找新目标或保持警戒
        return self._find_new_target_or_patrol(boat, observation)
    
    def _handle_target_frozen(self, target: BlackBoat) -> np.ndarray:
        """处理目标被冻结的情况"""
        print(f"[FSM] DEFENSIVE: 目标 {target.id} 已冻结，继续锁定击沉")
        return np.array([0.0, target.heading, target.id])
    
    def _handle_target_in_range(self, target: BlackBoat) -> np.ndarray:
        """处理目标在锁定范围内的情况"""
        print(f"[FSM] DEFENSIVE: 锁定目标 {target.id}，保持相同航向速度")
        return np.array([target.speed, target.heading, target.id])
    
    def _handle_target_out_of_range(self, boat: WhiteBoat, target: BlackBoat, observation: AgentObservation) -> np.ndarray:
        """处理目标超出锁定范围的情况"""
        print(f"[FSM] DEFENSIVE: 目标 {target.id} 超出锁定范围，追击")
        return self._pursue_target(boat, target, observation)
    
    def _remove_target_from_assignments(self, boat: WhiteBoat, target_id: str):
        """从分配列表中移除目标"""
        boat_target_list = self.boat_targets.get(boat.id, [])
        if isinstance(boat_target_list, list) and target_id in boat_target_list:
            boat_target_list.remove(target_id)
            self.boat_targets[boat.id] = boat_target_list
            print(f"[FSM] DEFENSIVE: 目标 {target_id} 已从船只 {boat.id} 的任务列表中移除")
        
        # 从探测目标中删除
        if target_id in self.detected_targets:
            del self.detected_targets[target_id]
    
    def _find_new_target_or_patrol(self, boat: WhiteBoat, observation: AgentObservation) -> np.ndarray:
        """寻找新目标或保持巡逻"""
        # 使用统一的目标选择逻辑
        target_id, action_type = self._find_best_target(boat, observation)
        if target_id:
            return self._engage_target(boat, target_id, action_type, observation)
        else:
            return self._maintain_defensive_position(boat, observation)
    
    def _pursue_target(self, boat: WhiteBoat, target: BlackBoat, observation: AgentObservation) -> np.ndarray:
        """追击目标"""
        # 计算追击航向
        intercept_result = self._calculate_intercept_point(boat, (target.x, target.y), target.speed, target.heading)
        if intercept_result:
            intercept_point, _, _ = intercept_result
            dx = intercept_point[0] - boat.x
            dy = intercept_point[1] - boat.y
            heading = math.atan2(dy, dx)
            speed = boat.max_speed
            return np.array([speed, heading, target.id])
        else:
            # 无法计算拦截点，保持防御位置
            print(f"[FSM] DEFENSIVE: Boat {boat.id} 无法计算目标 {target.id} 的拦截点")
            return self._maintain_defensive_position(boat, observation)

    def _find_target_by_id(self, target_id: str, observation: AgentObservation) -> Optional[BlackBoat]:
        """根据ID查找目标"""
        try:
            # 尝试将target_id转换为整数进行匹配
            target_id_int = int(target_id)
            return next((t for t in observation.black_boats if t.id == target_id_int), None)
        except ValueError:
            # 如果转换失败，尝试字符串匹配
            return next((t for t in observation.black_boats if str(t.id) == target_id), None)
    
    def _find_nearest_interceptable_target(self, boat: WhiteBoat, observation: AgentObservation) -> Optional[str]:
        """寻找最近的可拦截目标"""
        nearest_target_id = None
        min_intercept_time = float('inf')
        
        # 检查当前检测到的目标
        for target_id, target_data in self.detected_targets.items():
            if not target_data.get('active', True):
                continue
                
            # 使用预测位置计算拦截时间
            intercept_result = self._calculate_intercept_point(
                boat, target_data['position'], 
                target_data['speed'], 
                target_data['heading']
            )
            if intercept_result:
                _, intercept_time, _ = intercept_result
                if intercept_time < min_intercept_time:
                    min_intercept_time = intercept_time
                    nearest_target_id = target_id
        
        # 如果detected_targets中没有找到目标，检查observation.black_boats中的活跃目标
        if nearest_target_id is None:
            print(f"[FSM] DEFENSIVE: Boat {boat.id} 在detected_targets中没有找到可拦截目标，检查observation.black_boats")
            for target in observation.black_boats:
                if target.active:
                    # 计算拦截时间
                    intercept_result = self._calculate_intercept_point(
                        boat, (target.x, target.y), 
                        target.speed, 
                        target.heading
                    )
                    if intercept_result:
                        _, intercept_time, _ = intercept_result
                        if intercept_time < min_intercept_time:
                            min_intercept_time = intercept_time
                            nearest_target_id = str(target.id)
                            print(f"[FSM] DEFENSIVE: Boat {boat.id} 在observation.black_boats中找到可拦截目标 {target.id}")
        
        return nearest_target_id
    
    def _maintain_defensive_position(self, boat: WhiteBoat, observation: AgentObservation) -> np.ndarray:
        """保持防御位置 - 在原地警戒"""
        # 在DEFENSIVE阶段主动发射无人机增强探测
        drone_action = self._should_launch_drone_for_defense(boat, observation)
        
        # 检查目标列表是否为空
        boat_target_list = self.boat_targets.get(boat.id, [])
        if isinstance(boat_target_list, list) and not boat_target_list:
            print(f"[FSM] DEFENSIVE: Boat {boat.id} 目标列表已清空，在原地警戒")
        else:
            print(f"[FSM] DEFENSIVE: Boat {boat.id} 保持防御位置，增强探测")
        
        # 使用lock_id = -1表示不锁定任何目标
        return np.array([0.0, boat.heading, -1])
    
    def _should_launch_drone_for_defense(self, boat: WhiteBoat, observation: AgentObservation) -> float:
        """DEFENSIVE阶段是否应该发射无人机增强探测"""
        # 在DEFENSIVE阶段，主动发射无人机增强探测能力
        for drone in observation.all_drones:
            if (drone.docked_on_boat_id == boat.id and 
                drone.state == "recharging" ):
                print(f"[FSM] DEFENSIVE: Boat {boat.id} 主动发射无人机 {drone.id} 增强探测")
                return 1.0  # 发射无人机
        
        return 0.0  # 没有可用无人机
    
    def _should_launch_drone_for_tracking(self, boat: WhiteBoat, observation: AgentObservation) -> float:
        """检查是否应该发射无人机进行跟踪"""
        # 检查是否有威胁需要无人机支援

        threats = [t for t in observation.black_boats if t.active]
        if not threats:
            return 0.0  # 没有威胁，不发射
        
        # 有威胁，检查是否有可用的无人机
        for drone in observation.all_drones:
            if (drone.docked_on_boat_id == boat.id and 
                drone.state == "recharging"):
                print(f"[FSM] DEFENSIVE: Boat {boat.id} 发现威胁，发射无人机 {drone.id}")
                return 1.0  # 发射无人机
        
        return 0.0  # 没有可用无人机
    
    # ==================== 无人机相关函数 ====================
    
    def _generate_drone_action(self, drone: WhiteDrone, observation: AgentObservation) -> np.ndarray:
        """根据全局状态生成无人机动作"""
        # DEFENSIVE阶段：所有无人机可以起飞，跟踪或搜索
        if self.global_state == GlobalState.DEFENSIVE:
            return self._execute_defensive_drone_action(drone, observation)
        
        # 2号无人机执行任务，其余无人机PATROL阶段全部返航
        if self.global_state == GlobalState.PATROL:
            if drone.id == 2:
                return self._execute_patrol_drone_action(drone, observation)
            else:
                return self._drone_return_to_parent_action(drone, observation)
        
        # 其余阶段原有逻辑
        if self.global_state == GlobalState.DEPLOYMENT:
            return self._execute_deployment_drone_mission(drone, observation)
        else:
            return np.array([drone.min_speed, 1.57, 0.6])  # 保持飞行 (action >= 0.5)
    
    def _execute_defensive_drone_action(self, drone: WhiteDrone, observation: AgentObservation) -> np.ndarray:
        """
        执行DEFENSIVE阶段的无人机行为 - 完整的警戒扫视策略
        
        策略说明：
        - 无人机只有探测能力，无锁定和攻击能力
        - 无人船有20km探测范围，对40km内探测到的目标自动锁定
        
        策略流程：
        1. 进入DEFENSIVE阶段后，无人机先进行一次警戒扫视（以最小转弯半径飞一圈）
        2. 发现目标后根据距离和分配情况决定行为：
           ①目标在距离本无人船40km外，且在本舰本来的目标列表中：无人机持续跟踪监视目标直到击沉，然后回母舰充电
           ②目标在距离本无人船40km外，且不在本舰本来的目标列表中：无人机回母舰充电，无人船继续向着既定目标前进
           ③目标在距离本无人船40km内：无人机指引无人船发现目标（系统自动锁定），然后无人机回母舰充电
        3. 无人机在充电时要每隔600s执行一次警戒扫视
        4. 无人船完成对目标列表中目标的击沉后，把目标从自己的目标中删去，转向列表中的下一个目标
        5. 列表清空后，在原地警戒
        """
        current_time = observation.current_time
        drone_id = drone.id
        
        # 初始化无人机警戒状态
        if drone_id not in self.drone_alert_state:
            self.drone_alert_state[drone_id] = {
                'state': 'charging',
                'last_alert_time': 0,
                'target_id': None,
                'scan_center': None,
                'scan_angle': 0,
                'initial_scan_done': False  # 是否完成了进入DEFENSIVE阶段后的初始扫视
            }
        
        alert_state = self.drone_alert_state[drone_id]
        parent_boat = self._find_parent_boat(drone, observation)
        
        # 如果找不到母舰，保持当前状态
        if not parent_boat:
            return np.array([0.0, drone.heading, 1.0])
        
        # 能量检查 - 低于15%时强制返航
        if drone.energy < 15.0:
            alert_state['state'] = 'charging'
            print(f"[FSM] DEFENSIVE: 无人机 {drone_id} 能量不足({drone.energy:.1f}%)，返航充电")
            return self._drone_return_to_parent_action(drone, observation)
        
        # 如果无人机停靠在母舰上
        if drone.docked_on_boat_id != -1:
            # 检查是否需要进行警戒扫视
            time_since_last_alert = current_time - alert_state['last_alert_time']
            
            # 需要警戒扫视的条件：
            # 1. 初始扫视未完成，或
            # 2. 距离上次警戒扫视已过600秒
            need_alert_scan = (not alert_state['initial_scan_done'] or time_since_last_alert >= 600)
            
            if need_alert_scan:
                # 起飞进行警戒扫视
                alert_state['state'] = 'alert_scanning'
                alert_state['scan_center'] = (parent_boat.x, parent_boat.y)  # 以母舰位置为中心
                alert_state['scan_angle'] = 0
                alert_state['last_alert_time'] = current_time
                print(f"[FSM] DEFENSIVE: 无人机 {drone_id} 开始警戒扫视 (初始:{not alert_state['initial_scan_done']})")
                return np.array([20.0, drone.heading, 0.6])  # 起飞 (action >= 0.5)
            else:
                # 在充电，等待下次扫视时间
                return np.zeros(3)
        
        # 警戒扫视状态
        if alert_state['state'] == 'alert_scanning':
            return self._execute_alert_scanning(drone, alert_state, parent_boat, observation)
        
        # 监视目标状态 - 持续监视分配给母舰的远距离目标
        elif alert_state['state'] == 'monitoring':
            return self._execute_target_monitoring(drone, alert_state, parent_boat, observation)
        
        # 指引母舰锁定状态 - 指引母舰锁定近距离目标
        elif alert_state['state'] == 'guiding_lock':
            return self._execute_guiding_lock(drone, alert_state, parent_boat, observation)
        
        # 默认返回充电状态
        return self._drone_return_to_parent_action(drone, observation)

    def _assist_locking_operation(self, drone: WhiteDrone, target: BlackBoat, parent_boat: WhiteBoat, observation: AgentObservation) -> np.ndarray:
        """辅助母舰的锁定操作"""
        # 检查目标是否已被冻结
        if target.is_frozen(observation.current_time):
            print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 辅助锁定，目标 {target.id} 已冻结")
            return self._drone_return_to_parent_action(drone, observation)
        
        # 目标未冻结，继续辅助跟踪
        print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 辅助母舰锁定目标 {target.id}")
        return self._track_target_for_detection(drone, target)
    
    def _track_target_for_detection(self, drone: WhiteDrone, target: BlackBoat) -> np.ndarray:
        """跟踪目标以保持探测（无人机只负责探测，不攻击）"""
        dx = target.x - drone.x
        dy = target.y - drone.y
        distance = math.hypot(dx, dy)
        
        # 保持在合适的探测距离（不要太近也不要太远）
        ideal_distance = 3000  # 3km理想探测距离
        
        if distance < ideal_distance * 0.5:
            # 太近了，拉开距离以保持安全
            speed = drone.max_speed
            # 向目标相反方向移动
            heading = math.atan2(-dy, -dx)
            print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 距离目标过近，拉开距离")
        elif distance > ideal_distance * 2:
            # 太远了，接近目标以保持探测
            speed = drone.max_speed
            heading = math.atan2(dy, dx)
            print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 接近目标以保持探测")
        else:
            # 距离合适，保持跟随
            # 预测目标移动并跟随
            pred_x = target.x + target.speed * math.cos(target.heading) * 2  # 预测2秒后位置
            pred_y = target.y + target.speed * math.sin(target.heading) * 2
            dx_pred = pred_x - drone.x
            dy_pred = pred_y - drone.y
            heading = math.atan2(dy_pred, dx_pred)
            speed = min(drone.max_speed, target.speed * 1.2)  # 略快于目标
            print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 跟踪目标 {target.id} (距离: {distance:.0f}m)")
        
        return np.array([speed, heading, 1.0])  # 保持探测模式

    
    def _maintain_close_surveillance(self, drone: WhiteDrone, target: BlackBoat) -> np.ndarray:
        """对目标保持近距离监视（无人机只负责探测跟踪）"""
        dx = target.x - drone.x
        dy = target.y - drone.y
        distance = math.hypot(dx, dy)
        
        # 保持在合适距离监视目标
        ideal_distance = 30000  # 1km理想监视距离
        
        if distance > ideal_distance * 1.5:
            # 距离太远，接近目标
            heading = math.atan2(dy, dx)
            speed = min(drone.max_speed, distance / 5.0)
        elif distance < ideal_distance * 0.5:
            # 距离太近，拉开距离
            heading = math.atan2(-dy, -dx)
            speed = drone.max_speed * 0.5
        else:
            # 在合适距离，跟随目标移动
            # 预测目标位置并跟随
            pred_x = target.x + target.speed * math.cos(target.heading)
            pred_y = target.y + target.speed * math.sin(target.heading)
            dx_pred = pred_x - drone.x
            dy_pred = pred_y - drone.y
            heading = math.atan2(dy_pred, dx_pred)
            speed = target.speed
        
        print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 监视目标 {target.id} (距离: {distance:.0f}m)")
        return np.array([speed, heading, 1.0])
    

    
    def _execute_deployment_drone_mission(self, drone: WhiteDrone, observation: AgentObservation) -> np.ndarray:
        """
        DEPLOYMENT阶段无人机任务：航点侦察
        
        任务流程：
        1. 前往预定义的航点
        2. 在航点进行圆形扫描侦察
        3. 处理探测到的目标信息
        4. 前往下一个航点
        5. 完成所有航点后返回母舰
        
        状态管理：
        - going_to_first_target: 前往第一个航点
        - circle_scanning: 在航点进行圆形扫描
        - going_to_next_target: 前往下一个航点
        - returning: 返回母舰
        - mission_completed: 任务完成
        """
        # 初始化任务状态
        mission_state = self.drone_mission_state.setdefault(drone.id, {
            'phase': 'going_to_first_target',      # 当前阶段
            'current_target': 0,                   # 当前航点索引
            'circle_progress': 0.0,                # 圆形扫描进度
            'circle_center': None,                 # 扫描中心点
            'circle_radius': 100,                  # 扫描半径（米）
            'accumulated_angle': 0.0,              # 累计扫描角度
            'last_angle': None,                    # 上次扫描角度
            'detected_at_waypoint': []             # 在航点探测到的目标列表
        })
        
        # 能量检查：低于20%时立即返回
        if drone.energy < 20.0:
            mission_state['phase'] = 'returning'
        

        if mission_state['phase'] == 'returning':
            # 任务完成，标记状态
            mission_state['phase'] = 'mission_completed'
        # 如果停靠在母舰上，检查任务状态
        if drone.docked_on_boat_id != -1:
            if mission_state['phase'] == 'mission_completed':
                return np.zeros(3)
        
        # 获取无人机的航点列表
        waypoints = self.drone_waypoints.get(drone.id, [])
        if drone.id == 2:
            mission_state['phase'] = 'mission_completed'
            return np.array([0.0, 0.0, 0.0])
        # 执行航点任务
        return self._execute_waypoint_mission(drone, mission_state, waypoints, observation)
    
    def _execute_waypoint_mission(self, drone: WhiteDrone, mission_state: Dict, 
                                 waypoints: List[Tuple[float, float]], observation: AgentObservation) -> np.ndarray:
        """
        执行航点任务的具体逻辑
        
        阶段说明：
        1. going_to_first_target: 前往第一个航点
        2. circle_scanning: 在航点进行圆形扫描
        3. going_to_next_target: 前往下一个航点
        4. returning: 返回母舰
        
        Args:
            drone: 无人机对象
            mission_state: 任务状态字典
            waypoints: 航点列表
            observation: 环境观测
            
        Returns:
            np.ndarray: [速度, 航向, 无人机动作]
        """
        current_target = mission_state['current_target']
        phase = mission_state['phase']
        
        # 阶段1: 前往第一个航点
        if phase == 'going_to_first_target':
            if current_target < len(waypoints):
                target_x, target_y = waypoints[current_target]
                dx = target_x - drone.x
                dy = target_y - drone.y
                distance = math.sqrt(dx**2 + dy**2)
                
                if distance < 100:  # 到达航点（100米内）
                    # 切换到圆形扫描阶段
                    mission_state['phase'] = 'circle_scanning'
                    mission_state['circle_center'] = (target_x, target_y)
                    mission_state['circle_progress'] = 0.0
                    mission_state['accumulated_angle'] = 0.0
                    mission_state['last_angle'] = None
                    return np.array([drone.max_speed, math.atan2(dy, dx), 1.0])
                else:
                    # 继续向航点移动
                    return np.array([drone.max_speed, math.atan2(dy, dx), 1.0])
            else:
                # 没有更多航点，返回母舰
                mission_state['phase'] = 'returning'
                return self._drone_return_to_parent_action(drone, observation)
        
        # 阶段2: 圆形扫描
        elif phase == 'circle_scanning':
            scan_result = self._perform_circle_scan_fsm(drone, mission_state, observation)
            
            # 检查是否完成扫描
            if mission_state.get('scan_completed', False):
                # 完成当前航点扫描，处理探测到的目标
                self._process_detected_targets_at_waypoint(drone, mission_state, observation)
                
                # 前往下一个航点
                mission_state['current_target'] += 1
                mission_state['phase'] = 'going_to_next_target'
                mission_state['scan_completed'] = False
                
            return scan_result
        
        # 阶段3: 前往下一个航点
        elif phase == 'going_to_next_target':
            current_target = mission_state['current_target']
            if current_target < len(waypoints):
                target_x, target_y = waypoints[current_target]
                dx = target_x - drone.x
                dy = target_y - drone.y
                distance = math.sqrt(dx**2 + dy**2)

                if distance < 100:  # 到达下一个航点
                    # 切换到圆形扫描阶段
                    mission_state['phase'] = 'circle_scanning'
                    mission_state['circle_center'] = (target_x, target_y)
                    mission_state['circle_progress'] = 0.0
                    mission_state['accumulated_angle'] = 0.0
                    mission_state['last_angle'] = None
                    return np.array([drone.max_speed, math.atan2(dy, dx), 1.0])
                else:
                    # 继续向航点移动
                    return np.array([drone.max_speed, math.atan2(dy, dx), 1.0])
            else:
                # 没有更多航点，返回母舰
                mission_state['phase'] = 'returning'
                return self._drone_return_to_parent_action(drone, observation)
        
        # 阶段4: 返回母艇
        elif phase == 'returning' or phase == 'mission_completed':
            return self._drone_return_to_parent_action(drone, observation)
        
        # 默认行为
        return np.array([drone.max_speed, 0.0, 1.0])
    
    def _perform_circle_scan_fsm(self, drone: WhiteDrone, mission_state: Dict, observation: AgentObservation) -> np.ndarray:
        """
        执行圆形扫描（与simple_line_defense_agent相同的逻辑）
        """
        center_x, center_y = mission_state['circle_center']
        radius = mission_state['circle_radius']
        
        # 计算当前位置相对于圆心的角度
        current_angle = math.atan2(drone.y - center_y, drone.x - center_x)
        
        # 初始化累积角度
        if mission_state['last_angle'] is None:
            mission_state['last_angle'] = current_angle
        
        # 计算这一步转过的角度
        last_angle = mission_state['last_angle']
        angle_delta = current_angle - last_angle
        
        # 处理角度跳变
        if angle_delta > math.pi:
            angle_delta -= 2 * math.pi
        elif angle_delta < -math.pi:
            angle_delta += 2 * math.pi
        
        # 累积转过的角度
        if abs(angle_delta) < math.pi / 2:
            mission_state['accumulated_angle'] += abs(angle_delta)
        
        mission_state['last_angle'] = current_angle
        mission_state['circle_progress'] = mission_state['accumulated_angle'] / (2 * math.pi)
        
        # 检查是否完成一圈扫描 (降低完成阈值，加快扫描)
        if mission_state['circle_progress'] >= 0.85:  # 从0.99降低到0.85
            mission_state['scan_completed'] = True
        
        # 继续圆形运动
        tangent_angle = current_angle + math.pi / 2
        circle_speed = drone.max_speed * 1.0  # 提高扫描速度到最大速度
        
        return np.array([circle_speed, tangent_angle, 1.0])
    
    def _process_detected_targets_at_waypoint(self, drone: WhiteDrone, mission_state: Dict, observation: AgentObservation):
        """
        处理在航点探测到的目标，记录用于后续分组
        """
        detected_targets = observation.get_detected_targets()
        waypoint_targets = []
        
        for target in detected_targets:
            waypoint_targets.append({
                'id': target.id,
                'position': (target.x, target.y),
                'speed': getattr(target, 'speed', 0.0),
                'heading': getattr(target, 'heading', 0.0),
                'waypoint_index': mission_state['current_target'],
                'detection_time': observation.current_time
            })
        
        mission_state['detected_at_waypoint'].append(waypoint_targets)
        
        # 新增：拦截点逻辑
        self._update_boat_intercept_point_after_scan(drone, mission_state, waypoint_targets, observation)
        
        # TODO: 在这里进行目标分组（你提到等下再想分组策略）
        self._group_detected_targets(waypoint_targets)
        
        # TODO: 根据分组结果调整白方无人船航线（你提到等下再想）
        self._adjust_boat_routes_based_on_groups()
    
    def _update_boat_intercept_point_after_scan(self, drone: WhiteDrone, mission_state: Dict, waypoint_targets: list, observation: AgentObservation):
        """
        每次圆形扫描后，计算并更新母舰（无人船）的拦截点
        """
        # 只在第一次和第二次扫描后处理
        scan_index = mission_state['current_target']

        # 找到母舰
        parent_boat = None
        for boat in observation.white_boats:
            if boat.id == drone.parent_boat_id and boat.active:
                parent_boat = boat
                break
        if parent_boat is None:
            return
        # 找到x值最小的黑方无人船
        if not waypoint_targets:
            return  # 没有目标不变
        min_x_target = min(waypoint_targets, key=lambda t: t['position'][0])
        # 计算预计拦截点
        intercept_result = self._calculate_intercept_point(
            parent_boat,
            min_x_target['position'],
            min_x_target['speed'],
            min_x_target['heading']
        )
        
        # 检查拦截点是否有效
        if intercept_result is None:
            print(f"[FSM] Boat {parent_boat.id} 目标 {min_x_target['id']} 拦截点在封锁线后，放弃此目标")
            print(f"[FSM] 目标位置: {min_x_target['position']}, 速度: {min_x_target['speed']:.2f}, 航向: {min_x_target['heading']:.2f}")
            return

        intercept_point, intercept_time, boat_pos = intercept_result
        boat_id = parent_boat.id
        # 第一次扫描，强制更新
        if scan_index == 0:
            self.boat_intercept_info[boat_id] = {
                'target_id': min_x_target['id'],
                'intercept_point': intercept_point,
                'intercept_time': intercept_time,
                'scan_index': 0
            }
            # DEPLOYMENT阶段临时拦截点 - 仅用于DEPLOYMENT阶段，不影响PATROL阶段的正式分配
            self.boat_targets[boat_id] = intercept_point
            print(f"[FSM] Boat {boat_id} 第一次扫描后拦截目标 {min_x_target['id']}，拦截点 {intercept_point}，预计拦截时间 {intercept_time:.2f}")
        # 第二次扫描，若更早则替换
        elif scan_index == 1:
            prev = self.boat_intercept_info.get(boat_id)
            if prev is None or intercept_time < prev['intercept_time']:
                self.boat_intercept_info[boat_id] = {
                    'target_id': min_x_target['id'],
                    'intercept_point': intercept_point,
                    'intercept_time': intercept_time,
                    'scan_index': 1
                }
                # DEPLOYMENT阶段临时拦截点 - 仅用于DEPLOYMENT阶段，不影响PATROL阶段的正式分配
                self.boat_targets[boat_id] = intercept_point
                print(f"[FSM] Boat {boat_id} 第二次扫描后拦截目标 {min_x_target['id']}，拦截点 {intercept_point}，预计拦截时间 {intercept_time:.2f}（已更新）")
            else:
                print(f"[FSM] Boat {boat_id} 第二次扫描后拦截点未更新，仍为 {prev['intercept_point']}，预计拦截时间 {prev['intercept_time']:.2f}")

    # ==================== 计算工具函数 ====================
    
    def _calculate_intercept_point(self, boat, target_pos, target_speed, target_heading)-> Optional[Tuple[Tuple[float, float], float, Tuple[float, float]]]:
        """
        计算极限拦截点（考虑40000m锁定距离，不可拦截的返回None）
        boat: 白方无人船对象
        target_pos: (x, y)
        target_speed: float
        target_heading: float（弧度）
        返回：(intercept_x, intercept_y), intercept_time, (boat_x, boat_y) 或 None（如果无法拦截）
        """
        bx, by = boat.x, boat.y
        tx, ty = target_pos
        bv = boat.max_speed
        bh = boat.heading
        tv = target_speed
        th = target_heading
        dx = tx - bx
        dy = ty - by

        D=(dx*math.sin(th)-dy*math.cos(th))/(-dx*math.cos(th)+dy*math.sin(th))
        fx = lambda bh: D*(tv-bv*math.cos(th-bh))-(bv*math.sin(th-bh))
        root = opt.root(fx,bh)

        bh=root["x"][0]
        
        # 检查分母是否为0或接近0，避免除零错误
        denominator = bv * math.sin(th - bh)
        if abs(denominator) < 1e-6:  # 使用小的阈值避免数值不稳定
            print(f"[FSM] 拦截计算失败：分母接近0 (bv={bv:.2f}, th={th:.2f}, bh={bh:.2f}, sin(th-bh)={math.sin(th-bh):.6f})")
            return None
            
        intercept_time = (dx*math.sin(th)-dy*math.cos(th))/denominator
        # 确保时间非负
        if intercept_time <= 0:
            return None
        
        # 计算拦截点（目标在时间t后的位置）
        intercept_x = tx + tv * math.cos(th) * intercept_time
        intercept_y = ty + tv * math.sin(th) * intercept_time
        
        # 计算无人船在拦截时的位置
        boat_x = bx + bv * math.cos(th) * intercept_time
        boat_y = by + bv * math.sin(th) * intercept_time
        
        # 检查拦截点是否在封锁线后
        if intercept_x < self.blockade_line_x+3000:
            return None  # 拦截点在封锁线+3km后，放弃此目标
        
        return (intercept_x, intercept_y), intercept_time, (boat_x, boat_y)

    
    def _group_detected_targets(self, waypoint_targets: List[Dict]):
        """
        对探测到的目标进行分组
        TODO: 分组策略待实现
        """
        # 暂时空实现，等待你提供分组策略
        pass
    
    def _adjust_boat_routes_based_on_groups(self):
        """
        根据分组结果调整白方无人船航线
        TODO: 航线调整策略待实现
        """
        # 暂时空实现，等待你提供调整策略
        pass
    
    # 无人艇动作实现
    # ==================== DEPLOYMENT阶段函数 ====================
    
    def _execute_deployment_boat_action(self, boat: WhiteBoat, observation: AgentObservation) -> np.ndarray:
        """
        DEPLOYMENT阶段无人船动作：向部署线移动
        
        策略：
        - 计算无人船在部署线上的目标位置
        - 以最大速度向目标位置移动
        - 不锁定任何目标（lock_id = -1）
        """
        # 计算部署线上的目标位置（根据船只索引均匀分布）
        target_pos = self._get_deployment_position(boat, observation.get_active_boats())
        
        # 计算移动方向和速度
        dx = target_pos[0] - boat.x
        dy = target_pos[1] - boat.y
        heading = math.atan2(dy, dx)
        speed = boat.max_speed  # 以最大速度移动
        
        # 不锁定任何目标
        return np.array([speed, heading, -1])

    def _is_drone_ready_on_boat(self, boat: WhiteBoat, observation: AgentObservation) -> bool:
        """
        检查无人船上是否有可发射的无人机
        
        条件：
        - 无人机停靠在该船上
        - 无人机状态为充电中（recharging）
        """
        for drone in observation.all_drones:
            if (drone.docked_on_boat_id == boat.id and 
                drone.state == "recharging"):
                return True
        return False
    
    def _get_deployment_position(self, boat: WhiteBoat, active_boats: List[WhiteBoat]) -> Tuple[float, float]:
        """
        计算无人船在部署线上的目标位置
        
        部署策略：
        - 所有船只沿部署线（x=60000）均匀分布
        - 第一艘船在部署线起点，最后一艘船在部署线终点
        - 其他船只在两点之间均匀分布
        
        Args:
            boat: 目标船只
            active_boats: 活跃船只列表
            
        Returns:
            (x, y): 部署线上的目标位置
        """
        try:
            boat_index = active_boats.index(boat)
            total_boats = len(active_boats)
        except ValueError:
            # 如果船只不在活跃列表中，返回部署线中点
            mid_y = (self.deployment_line_y_start + self.deployment_line_y_end) / 2
            return (self.deployment_line_x, mid_y)
        
        # 计算部署位置
        if total_boats == 1:
            # 只有一艘船时，放在部署线中点
            target_y = (self.deployment_line_y_start + self.deployment_line_y_end) / 2
        else:
            # 多艘船时，在部署线上均匀分布
            ratio = boat_index / (total_boats - 1)  # 0到1之间的比例
            target_y = self.deployment_line_y_start + ratio * (self.deployment_line_y_end - self.deployment_line_y_start)
        
        target_x = self.deployment_line_x
        
        return (target_x, target_y)
    
    def _boat_hold_position(self, boat: WhiteBoat, observation: AgentObservation) -> np.ndarray:
        """保持位置"""
        lock_action = self._should_lock_target(boat, observation)
        
        # 在defensive阶段，主动发射无人机增强探测
        drone_action = 0.0
        if self.global_state == GlobalState.DEFENSIVE:
            for drone in observation.all_drones:
                if (drone.docked_on_boat_id == boat.id and 
                    drone.state == "recharging" and 
                    drone.energy >= 10.0):
                    drone_action = 1.0
                    print(f"[FSM] DEFENSIVE阶段：Boat {boat.id} 在保持位置时发射无人机 {drone.id} 增强探测")
                    break
        else:
            drone_action = self._should_launch_drone(boat, observation)
            
        # 使用lock_id = -1表示不锁定任何目标
        return np.array([0.0, boat.heading, -1])
    
    # def _boat_pursue_target(self, boat: WhiteBoat, observation: AgentObservation) -> np.ndarray:
    #     """追击目标"""
    #     target_pos = self.boat_targets.get(boat.id)
    #     if not target_pos:
    #         return self._boat_hold_position(boat, observation)
        
       
        
    #     # 处理多目标情况：选择第一个目标作为主要目标
    #     if isinstance(target_pos, list) and len(target_pos) > 0:
    #         # 多目标情况，选择第一个目标
    #         primary_target = target_pos[0]
    #         if isinstance(primary_target, (list, tuple, np.ndarray)):
    #             target_x, target_y = primary_target[0], primary_target[1]
    #         else:
    #             target_x, target_y = primary_target, primary_target
    #     elif isinstance(target_pos, (tuple, np.ndarray)):
    #         # 单目标情况
    #         target_x, target_y = target_pos[0], target_pos[1]
    #     else:
    #         # 其他情况，保持位置
    #         return self._boat_hold_position(boat, observation)
        
    #     # 重新计算拦截点：从预测位置反推找到对应的目标信息，然后计算拦截点
    #     intercept_point = None
    #     for unit_id, info in self.detected_targets.items():
    #         x, y = info['position']
    #         speed = info.get('speed', 0.0)
    #         heading = info.get('heading', 0.0)
    #         last_seen = info.get('last_seen', observation.current_time)
    #         dt = observation.current_time - last_seen
    #         pred_x = x + speed * math.cos(heading) * dt
    #         pred_y = y + speed * math.sin(heading) * dt
            
    #         # 检查这个预测位置是否匹配当前目标（放宽匹配条件）
    #         if abs(pred_x - target_x) < 1000.0 and abs(pred_y - target_y) < 1000.0:
    #             # 找到匹配的目标，计算拦截点
    #             intercept_result = self._calculate_intercept_point(boat, info['position'], speed, heading)
    #             if intercept_result is not None:
    #                 intercept_point, intercept_time, boat_pos = intercept_result
    #                 print(f"[FSM] 舰船{boat.id} 目标unit_id={unit_id} 预测位置: ({target_x:.0f}, {target_y:.0f}) -> 拦截点: ({intercept_point[0]:.0f}, {intercept_point[1]:.0f})")
    #             break
        
    #     # 如果找到拦截点，使用拦截点；否则使用原始目标位置
    #     if intercept_point is not None:
    #         dx = intercept_point[0] - boat.x
    #         dy = intercept_point[1] - boat.y
    #         print(f"[FSM] 舰船{boat.id} 使用拦截点: ({intercept_point[0]:.0f}, {intercept_point[1]:.0f})")
    #     else:
    #         dx = target_x - boat.x
    #         dy = target_y - boat.y
    #         print(f"[FSM] 舰船{boat.id} 未找到拦截点，使用原始目标位置: ({target_x:.0f}, {target_y:.0f})")
            
    #     distance = math.hypot(dx, dy)
        
    #     if distance < 100:
    #         # 到达后尝试锁定，需要找到目标ID
    #         target_id = -1
    #         for unit_id, info in self.detected_targets.items():
    #             x, y = info['position']
    #             speed = info.get('speed', 0.0)
    #             heading = info.get('heading', 0.0)
    #             last_seen = info.get('last_seen', observation.current_time)
    #             dt = observation.current_time - last_seen
    #             pred_x = x + speed * math.cos(heading) * dt
    #             pred_y = y + speed * math.sin(heading) * dt
                
    #             # 检查这个预测位置是否匹配当前目标
    #             if abs(pred_x - target_x) < 1000.0 and abs(pred_y - target_y) < 1000.0:
    #                 target_id = int(unit_id)
    #                 break
            
    #         return np.array([0.0, boat.heading, target_id])  # 到达后尝试锁定指定目标
        
    #     heading = math.atan2(dy, dx)
    #     speed = boat.max_speed
        
    #     # 判断是否发射无人机（在defensive阶段主动发射无人机增强探测）
    #     drone_action = 0.0
    #     for drone in observation.all_drones:
    #         if drone.docked_on_boat_id == boat.id and drone.state == "recharging" and drone.energy >= 90.0:
    #             # 在defensive阶段，主动发射无人机增强探测
    #             if self.global_state == GlobalState.DEFENSIVE:
    #                 drone_action = 1.0
    #                 print(f"[FSM] DEFENSIVE阶段：Boat {boat.id} 在追击目标时发射无人机 {drone.id} 增强探测")
    #                 break
    #             # 2号无人机特殊逻辑：任务分配前不发射
    #             elif drone.id == 2 and not getattr(self, '_final_assignment_done', False):
    #                 continue
    #             else:
    #                 drone_action = 1.0
    #                 break
        
    #     # 需要找到目标ID用于锁定
    #     target_id = -1
    #     for unit_id, info in self.detected_targets.items():
    #         x, y = info['position']
    #         speed_target = info.get('speed', 0.0)
    #         heading_target = info.get('heading', 0.0)
    #         last_seen = info.get('last_seen', observation.current_time)
    #         dt = observation.current_time - last_seen
    #         pred_x = x + speed_target * math.cos(heading_target) * dt
    #         pred_y = y + speed_target * math.sin(heading_target) * dt
            
    #         # 检查这个预测位置是否匹配当前目标
    #         if abs(pred_x - target_x) < 1000.0 and abs(pred_y - target_y) < 1000.0:
    #             target_id = int(unit_id)
    #             break
        
    #     return np.array([speed, heading, target_id])
    
    def _boat_lock_target(self, boat: WhiteBoat, observation: AgentObservation) -> np.ndarray:
        """锁定目标"""
        detected_targets = observation.get_detected_targets()
        
        if not detected_targets:
            print(f"[FSM] 舰船{boat.id} 没有探测到任何目标，无法锁定")
            return np.array([0.0, boat.heading, -1])
        
        print(f"[FSM] 舰船{boat.id} 开始锁定检查，探测到 {len(detected_targets)} 个目标")
        
        # 找到最近的可锁定目标
        nearest_lockable_target = None
        min_distance = float('inf')
        
        for target in detected_targets:
            distance = boat.distance_to(target)
            print(f"[FSM] 舰船{boat.id} 检查目标 unit_id={target.id}，距离: {distance:.0f}m，锁定范围: {GameConfig.WHITE_BOAT_LOCK_RANGE}m")
            
            if distance <= GameConfig.WHITE_BOAT_LOCK_RANGE:
                if distance < min_distance:
                    nearest_lockable_target = target
                    min_distance = distance
                    print(f"[FSM] 舰船{boat.id} 更新最近可锁定目标为 unit_id={target.id}")
        
        if nearest_lockable_target:
            print(f"[FSM] 舰船{boat.id} 正在锁定目标 unit_id={nearest_lockable_target.id}，距离: {min_distance:.0f}m")
            
            # 找到目标在探测列表中的索引
            target_index = -1
            for i, target in enumerate(detected_targets):
                if target.id == nearest_lockable_target.id:
                    target_index = i
                    break
            
            if target_index >= 0:
                print(f"[FSM] 舰船{boat.id} 目标 unit_id={nearest_lockable_target.id} 在探测列表中的索引为 {target_index}")
                # 在锁定范围内，保持与目标相同航向速度并锁定
                return np.array([nearest_lockable_target.speed, nearest_lockable_target.heading, target_index])
            else:
                print(f"[FSM] 舰船{boat.id} 无法找到目标 unit_id={nearest_lockable_target.id} 在探测列表中的索引")
                return np.array([0.0, boat.heading, -1])
        
        # 如果没有可锁定的目标，返回保持位置
        print(f"[FSM] 舰船{boat.id} 没有可锁定的目标（所有目标都在锁定范围外）")
        return np.array([0.0, boat.heading, -1])
    
    # 无人机动作实现

    
    def _create_drone_patrol_path(self, observation: AgentObservation   ):
        # 2号无人机TSP路径生成 ======
                
        # 获取所有黑方目标的预测位置和ID
        points_with_ids = self._all_interceptable_black_ids(observation)
        if not points_with_ids:
            print("[FSM] 2号无人机TSP路径生成失败：没有可拦截的目标")
        elif len(points_with_ids) >= 2:
            try:
             # 提取坐标点用于TSP求解
                points = [point_info['position'] for point_info in points_with_ids]
                order, cost = solve_tsp_in_memory(points)
                    
                # 检查order是否为None
                if order is None:
                    print("[FSM] 2号无人机TSP路径生成失败：order为None")
                else:
                    # 根据TSP顺序重新排列目标ID
                    tsp_id_order = [points_with_ids[i]['target_id'] for i in order]
                    self._drone2_tsp_path = tsp_id_order
                    print(f"[FSM] 2号无人机TSP路径生成完成，目标id顺序: {tsp_id_order}, 路径长度: {cost:.2f}")
            except Exception as e:
                print(f"[FSM] 2号无人机TSP路径生成失败: {e}")
        else:
            print(f"[FSM] 2号无人机TSP路径生成失败：可拦截目标点数不足2（当前: {len(points_with_ids)}）")

    def _drone_return_action(self, drone: WhiteDrone, observation: AgentObservation) -> np.ndarray:
        """返航动作"""
        parent_boat = self._find_parent_boat(drone, observation)
        if parent_boat:
            dx = parent_boat.x - drone.x
            dy = parent_boat.y - drone.y
            heading = math.atan2(dy, dx)
            return np.array([drone.max_speed, heading, 0.1])  # 降落到起飞母舰
        return np.array([drone.min_speed, drone.heading, 0.3])  # 降落到最近母舰
    
    def _drone_emergency_return_action(self, drone: WhiteDrone, observation: AgentObservation) -> np.ndarray:
        """紧急返航"""
        return self._drone_return_action(drone, observation)
    

    def _find_nearest_high_threat_target(self, boat: WhiteBoat) -> Optional[Tuple[float, float]]:
        """寻找最近的高威胁目标"""
        high_threat_targets = [
            info['position'] for info in self.detected_targets.values()
            if info['threat_level'] > 0.6
        ]
        
        if not high_threat_targets:
            return None
        
        distances = [
            math.hypot(pos[0] - boat.x, pos[1] - boat.y)
            for pos in high_threat_targets
        ]
        
        min_index = distances.index(min(distances))
        return high_threat_targets[min_index]
    
    def _should_lock_target(self, boat: WhiteBoat, observation: AgentObservation) -> float:
        """判断是否应该锁定目标"""
        detected_targets = observation.get_detected_targets()
        for target in detected_targets:
            distance = boat.distance_to(target)
            if distance <= GameConfig.WHITE_BOAT_LOCK_RANGE:
                print(f"[FSM] 舰船{boat.id} 发现可锁定目标 unit_id={target.id}，距离: {distance:.0f}m，锁定范围: {GameConfig.WHITE_BOAT_LOCK_RANGE}m")
                return 1.0
        return 0.0
    
    def _should_launch_drone(self, boat: WhiteBoat, observation: AgentObservation) -> float:
        """判断是否应该发射无人机"""
        for drone in observation.all_drones:
            if (drone.docked_on_boat_id == boat.id and 
                drone.state == "recharging" and 
                drone.energy >= 10.0):
                return 1.0
        return 0.0
    
    def _get_next_patrol_point(self, drone: WhiteDrone, area: Dict) -> Tuple[float, float]:
        """获取下一个巡航点"""
        points = area['patrol_points']
        # 简单实现：选择最近的点
        distances = [
            math.hypot(point[0] - drone.x, point[1] - drone.y)
            for point in points
        ]
        min_index = distances.index(min(distances))
        return points[(min_index + 1) % len(points)]
    
    def _find_parent_boat(self, drone: WhiteDrone, observation: AgentObservation) -> Optional[WhiteBoat]:
        """找到无人机的母舰"""
        # 首先查找无人机停靠的船只
        if drone.docked_on_boat_id is not None:
            for boat in observation.white_boats:
                if boat.id == drone.docked_on_boat_id and boat.active:
                    return boat
        
        # 如果没有停靠信息，查找parent_boat_id
        if hasattr(drone, 'parent_boat_id') and drone.parent_boat_id is not None:
            for boat in observation.white_boats:
                if boat.id == drone.parent_boat_id and boat.active:
                    return boat
        
        # 最后查找最近的活跃船只作为母舰
        active_boats = [boat for boat in observation.white_boats if boat.active]
        if active_boats:
            nearest_boat = min(active_boats, 
                             key=lambda b: math.hypot(b.x - drone.x, b.y - drone.y))
            return nearest_boat
        
        return None
    
    def _drone_return_to_parent_action(self, drone: WhiteDrone, observation: AgentObservation) -> np.ndarray:
        """
        无人机返回母舰的动作（从simple_line_defense_agent复制）
        """
        # 寻找母舰
        parent_boat = None
        for boat in observation.white_boats:
            if (boat.id == drone.parent_boat_id and 
                boat.active and 
                not boat.is_frozen(observation.current_time)):
                parent_boat = boat
                break
        
        if parent_boat is None:
            # 没有找到母舰，使用最近母舰策略
            return np.array([drone.min_speed, drone.heading, 0.3])
        
        # 计算返回方向
        dx = parent_boat.x - drone.x
        dy = parent_boat.y - drone.y
        distance = math.sqrt(dx**2 + dy**2)
        
        
        heading = math.atan2(dy, dx)
        
        # 检查是否接近母舰，如果很近则切换到降落模式
        if distance < 5000:  # 5km内准备降落
            return np.array([drone.max_speed, heading, 0.1])  # 降落到起飞母舰 (action < 0.25)
        else:
            return np.array([drone.max_speed, heading, 0.8])  # 飞行模式 (action >= 0.5) - 保持探测

    def _GroupBlackTargets(self, targets: List[Tuple[int, float, float]]) -> Dict[int, List[Dict]]:
        """
        对黑方无人船目标进行分组
        1
        """
        grouped = {}
        for target in targets:
            drone_id = target[0]
            if drone_id not in grouped:
                grouped[drone_id] = []
            grouped[drone_id].append({
                'position': (target[1], target[2]),
                'velocity': (0.0, 0.0),  # 默认速度为0
            })
        return grouped

    
    def _all_interceptable_black_ids(self, observation: AgentObservation):
        """
        获取所有可拦截的黑方目标的预测位置和ID
        """
        now = observation.current_time
        points_with_ids = []
        
        # 收集白方无人船当前位置用于拦截点计算
        white_boats = [boat for boat in observation.white_boats if boat.active]
        
        for unit_id, info in self.detected_targets.items():
            x, y = info['position']
            speed = info.get('speed', 0.0)
            heading = info.get('heading', 0.0)
            last_seen = info.get('last_seen', now)
            dt = now - last_seen
            pred_x = x + speed * math.cos(heading) * dt
            pred_y = y + speed * math.sin(heading) * dt
            
            # 检查是否有任何白方无人船可以拦截此目标
            can_intercept = False
            for boat in white_boats:
                intercept_result = self._calculate_intercept_point(
                    boat, info['position'], speed, heading
                )
                if intercept_result is not None:
                    can_intercept = True
                    break
            
            # 只有可以拦截的目标才加入TSP路径
            if can_intercept:
                points_with_ids.append({
                    'target_id': unit_id,  # 使用unit_id
                    'position': [pred_x, pred_y],
                    'original_position': (x, y),
                    'speed': speed,
                    'heading': heading
                })
        
        print(f"[FSM] 2号无人机TSP路径包含 {len(points_with_ids)} 个可拦截目标")
        if points_with_ids:
            min_y = min(p['position'][1] for p in points_with_ids)
            max_y = max(p['position'][1] for p in points_with_ids)
            print(f"[FSM] 目标y坐标范围: {min_y:.0f} ~ {max_y:.0f}")
            print(f"[FSM] TSP路径目标ID列表: {[p['target_id'] for p in points_with_ids]}")
        
        return points_with_ids

    # 只在分配/切换时打印一次目标信息
    def _print_boat_targets_once(self):
        """打印无人船目标分配情况（仅执行一次）"""
        if not hasattr(self, '_targets_printed'):
            print("\n=== 无人船目标分配情况 ===")
            for boat_id, targets in self.boat_targets.items():
                print(f"Boat {boat_id}: {targets}")
            print("========================\n")
            self._targets_printed = True
    
    def _debug_defensive_phase(self, observation: AgentObservation):
        """调试defensive阶段的状态和锁定情况"""
        pass
    
    def _continue_alert_scanning(self, drone: WhiteDrone, alert_state: dict) -> np.ndarray:
        """继续警戒扫视 - 以最小转弯半径飞一圈"""
        center_x, center_y = alert_state['scan_center']
        current_angle = alert_state['scan_angle']
        
        # 计算最小转弯半径（根据无人机性能参数）
        min_turn_radius = 200  # 最小转弯半径200m
        
        # 计算圆周上的下一个点
        angle_increment = 0.05  # 角度增量，控制扫视速度
        next_angle = current_angle + angle_increment
        target_x = center_x + min_turn_radius * math.cos(next_angle)
        target_y = center_y + min_turn_radius * math.sin(next_angle)
        
        # 更新扫视角度
        alert_state['scan_angle'] = next_angle
        
        # 如果完成一圈，回到充电状态
        if next_angle >= 2 * math.pi:
            alert_state['state'] = 'charging'
            print(f"[FSM] DEFENSIVE: 无人机警戒扫视完成，返航充电")
            # 计算返回母舰的方向
            parent_x, parent_y = center_x, center_y  # 起飞点作为返回点
            dx = parent_x - drone.x
            dy = parent_y - drone.y
            heading = math.atan2(dy, dx)
            return np.array([drone.max_speed, heading, 1.0])
        
        # 计算朝向扫视点的方向
        dx = target_x - drone.x
        dy = target_y - drone.y
        heading = math.atan2(dy, dx)
        
        return np.array([drone.max_speed, heading, 1.0])
    
    def _track_target_for_detection_only(self, drone: WhiteDrone, target) -> np.ndarray:
        """无人机跟踪目标进行探测（无攻击能力）"""
        # 计算跟踪方向
        dx = target.x - drone.x
        dy = target.y - drone.y
        distance = math.hypot(dx, dy)
        
        # 保持适当距离进行探测跟踪
        ideal_distance = 30000  # 30km理想探测距离

        if distance > ideal_distance :
            # 距离太远，快速接近
            speed = drone.max_speed
            heading = math.atan2(dy, dx)
        elif distance < ideal_distance * 0.5:
            # 距离太近，稍微拉开距离
            speed = drone.max_speed * 0.5
            heading = math.atan2(-dy, -dx)
        else:
            # 距离合适，跟随目标
            # 预测目标移动方向并跟随
            pred_x = target.x + target.speed * math.cos(target.heading) * 3
            pred_y = target.y + target.speed * math.sin(target.heading) * 3
            dx_pred = pred_x - drone.x
            dy_pred = pred_y - drone.y
            heading = math.atan2(dy_pred, dx_pred)
            speed = max(drone.max_speed * 0.7, target.speed)
        
        print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 跟踪探测目标 {target.id} (距离: {distance:.0f}m)")
        return np.array([speed, heading, 1.0])
    
    def _is_target_assigned_to_boat(self, target_id: str) -> bool:
        """检查目标是否已被无人船分配"""
        for boat_id, targets in self.boat_targets.items():
            if target_id in targets:
                return True
        return False
    
    def _assign_target_to_nearest_boat(self, target_id: str, target, observation: AgentObservation):
        """将目标分配给最近的无人船"""
        boats = observation.white_boats
        if not boats:
            return
        
        # 找到最近的无人船
        nearest_boat = min(boats, 
                          key=lambda b: math.hypot(b.x - target.x, b.y - target.y))
        
        boat_id = nearest_boat.id
        if boat_id not in self.boat_targets:
            self.boat_targets[boat_id] = []
        
        # 如果目标不在分配列表中，添加到末尾
        if target_id not in self.boat_targets[boat_id]:
            self.boat_targets[boat_id].append(target_id)
            print(f"[FSM] DEFENSIVE: 将目标 {target_id} 分配给无人船 {boat_id}")
    
    def _get_visible_targets_for_drone(self, drone: WhiteDrone, observation: AgentObservation):
        """获取无人机可见的目标"""
        visible_targets = []
        black_boats = observation.black_boats
        
        for target in black_boats:
            if not target.active:
                continue
                
            # 计算距离
            distance = math.hypot(target.x - drone.x, target.y - drone.y)
            
            # 检查是否在探测范围内
            if distance <= GameConfig.WHITE_DRONE_DETECT_RANGE:
                # 检查是否在探测角度内（60度扇形）
                dx = target.x - drone.x
                dy = target.y - drone.y
                target_angle = math.atan2(dy, dx)
                
                # 计算角度差
                angle_diff = target_angle - drone.heading
                
                # 标准化角度差到[-π, π]
                while angle_diff > math.pi:
                    angle_diff -= 2 * math.pi
                while angle_diff < -math.pi:
                    angle_diff += 2 * math.pi
                
                # 检查是否在60°扇形内（±30°）
                half_sector_angle = math.radians(GameConfig.WHITE_DRONE_DETECT_ANGLE / 2)
                if abs(angle_diff) <= half_sector_angle:
                    visible_targets.append(target)
        
        return visible_targets
    


    def _is_target_being_locked(self, observation: AgentObservation) -> bool:
        """
        检查是否有目标正在被锁定
        
        Args:
            observation: 当前观察
            
        Returns:
            bool: 是否有目标正在被锁定
        """
        for boat in observation.white_boats:
            if boat.active and boat.is_locking and boat.current_target:
                return True
        return False
    
    def _get_locked_target_heading(self, observation: AgentObservation) -> Optional[float]:
        """
        获取被锁定目标的航向
        
        Args:
            observation: 当前观察
            
        Returns:
            Optional[float]: 被锁定目标的航向，如果没有则返回None
        """
        for boat in observation.white_boats:
            if boat.active and boat.is_locking and boat.current_target:
                return boat.current_target.heading
        return None

    def _execute_alert_scanning(self, drone: WhiteDrone, alert_state: dict, parent_boat: WhiteBoat, observation: AgentObservation) -> np.ndarray:
        """执行警戒扫视 - 以最小转弯半径飞一圈并在发现目标时做出判断"""
        center_x, center_y = alert_state['scan_center']
        current_angle = alert_state['scan_angle']
        
        # 计算最小转弯半径
        min_turn_radius = 500  # 1km转弯半径，确保较大的搜索范围
        
        # 在扫视过程中检查是否发现目标
        visible_targets = self._get_visible_targets_for_drone(drone, observation)
        
        if visible_targets:
            # 选择最近的目标
            closest_target = min(visible_targets, key=lambda t: math.hypot(t.x - drone.x, t.y - drone.y))
            dist_to_boat = math.hypot(closest_target.x - parent_boat.x, closest_target.y - parent_boat.y)
            threshold = 40000.0  # 40km阈值
            
            # 检查目标是否在母舰的目标列表中
            boat_target_list = self.boat_targets.get(parent_boat.id, [])
            target_in_list = str(closest_target.id) in boat_target_list if isinstance(boat_target_list, list) else False
            
            print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 在警戒扫视中发现目标 {closest_target.id}")
            print(f"                距离母舰: {dist_to_boat:.0f}m, 在目标列表中: {target_in_list}")
            
            # 情况1：距离>40km 且在本舰目标列表中 - 持续监视直到击沉
            if dist_to_boat > threshold and target_in_list:
                alert_state['state'] = 'monitoring'
                alert_state['target_id'] = str(closest_target.id)
                alert_state['initial_scan_done'] = True
                print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 开始监视目标 {closest_target.id} (>40km, 已分配)")
                return self._track_target_for_detection(drone, closest_target)
            
            # 情况2：距离>40km 且不在本舰目标列表中 - 返回充电
            elif dist_to_boat > threshold and not target_in_list:
                alert_state['state'] = 'charging'
                alert_state['initial_scan_done'] = True
                print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 发现目标 {closest_target.id} (>40km, 非本舰目标)，返回充电")
                return self._drone_return_to_parent_action(drone, observation)
            
            # 情况3：距离<=40km - 指引母舰发现目标（系统会自动锁定）
            else:
                alert_state['state'] = 'guiding_lock'
                alert_state['target_id'] = str(closest_target.id)
                alert_state['initial_scan_done'] = True
                # 通知母舰有近距离目标
                self.drone_close_target_alerts[parent_boat.id] = {
                    'target_id': str(closest_target.id),
                    'alert_time': observation.current_time,
                    'handled': False
                }
                print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 发现近距离目标 {closest_target.id} (<=40km)，指引母舰发现目标")
                return self._guide_boat_to_target(drone, closest_target, parent_boat)
        
        # 继续扫视 - 以最小转弯半径飞一圈
        angle_increment = 0.1  # 角度增量，控制扫视精度
        next_angle = current_angle + angle_increment
        
        # 计算圆周上的下一个点
        target_x = center_x + min_turn_radius * math.cos(next_angle)
        target_y = center_y + min_turn_radius * math.sin(next_angle)
        
        # 更新扫视角度
        alert_state['scan_angle'] = next_angle
        
        # 如果完成一圈，返回充电
        if next_angle >= 2 * math.pi:
            alert_state['state'] = 'charging'
            alert_state['initial_scan_done'] = True
            print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 警戒扫视完成，返航充电")
            return self._drone_return_to_parent_action(drone, observation)
        
        # 朝向下一个扫视点
        dx = target_x - drone.x
        dy = target_y - drone.y
        heading = math.atan2(dy, dx)
        
        return np.array([drone.max_speed * 0.7, heading, 1.0])  # 稍慢的速度进行扫视

    def _execute_target_monitoring(self, drone: WhiteDrone, alert_state: dict, parent_boat: WhiteBoat, observation: AgentObservation) -> np.ndarray:
        """执行目标监视 - 持续监视直到目标击沉（无人机只负责探测跟踪）"""
        target_id = alert_state['target_id']
        target = self._find_target_by_id(target_id, observation)
        
        if not target or not target.active:
            # 目标消失或被击沉，从母舰的目标列表中移除并转向下一个目标
            self._handle_target_destroyed_notification(target_id, parent_boat.id, observation.current_time)
            alert_state['state'] = 'charging'
            alert_state['target_id'] = None
            print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 监视的目标 {target_id} 已被击沉，返航充电")
            return self._drone_return_to_parent_action(drone, observation)
        
        # 继续监视目标直到击沉（无人机只负责跟踪探测，不攻击）
        print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 持续跟踪监视目标 {target_id}")
        return self._track_target_for_detection(drone, target)

    def _execute_guiding_lock(self, drone: WhiteDrone, alert_state: dict, parent_boat: WhiteBoat, observation: AgentObservation) -> np.ndarray:
        """执行指引母舰发现目标（系统会自动锁定40km内的目标）"""
        target_id = alert_state['target_id']
        target = self._find_target_by_id(target_id, observation)
        
        if not target or not target.active:
            # 目标消失，结束指引
            alert_state['state'] = 'charging'
            alert_state['target_id'] = None
            print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 指引的目标 {target_id} 已消失，返航充电")
            return self._drone_return_to_parent_action(drone, observation)
        
        # 检查目标是否已经进入母舰40km锁定范围（系统会自动锁定）
        dist_to_boat = math.hypot(target.x - parent_boat.x, target.y - parent_boat.y)
        if dist_to_boat <= 40000:  # 40km内
            # 检查母舰是否已经锁定了目标
            if parent_boat.current_target and str(parent_boat.current_target.id) == target_id:
                if parent_boat.is_locking:
                    # 母舰已锁定，无人机返航
                    alert_state['state'] = 'charging'
                    alert_state['target_id'] = None
                    # 标记近距离目标警报为已处理
                    if parent_boat.id in self.drone_close_target_alerts:
                        self.drone_close_target_alerts[parent_boat.id]['handled'] = True
                    print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 母舰已锁定目标 {target_id}，返航充电")
                    return self._drone_return_to_parent_action(drone, observation)
        
        # 继续指引母舰发现目标
        return self._guide_boat_to_target(drone, target, parent_boat)
                

    def _guide_boat_to_target(self, drone: WhiteDrone, target: BlackBoat, parent_boat: WhiteBoat) -> np.ndarray:
        """指引母舰发现目标（无人机只负责探测指引，系统会自动锁定）"""
        # 无人机在目标附近保持探测，让母舰能够发现目标
        # 计算一个跟随目标的位置（保持探测距离）
        follow_distance = 2000  # 跟随距离2km，确保母舰能探测到
        
        # 计算相对于目标的位置（在目标侧方，便于母舰探测）
        target_heading = target.heading
        # 在目标侧方保持位置，既能探测目标又能被母舰探测到
        follow_x = target.x + follow_distance * math.cos(target_heading + math.pi/2)
        follow_y = target.y + follow_distance * math.sin(target_heading + math.pi/2)
        
        # 朝向跟随位置
        dx = follow_x - drone.x
        dy = follow_y - drone.y
        heading = math.atan2(dy, dx)
        
        print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 指引母舰发现目标 {target.id}")
        return np.array([drone.max_speed * 0.8, heading, 1.0])

    def _handle_target_destroyed_notification(self, target_id: str, responsible_boat_id: int, destruction_time: float):
        """处理目标被击沉的通知"""
        # 记录击沉通知
        self.target_destruction_notifications.append((target_id, responsible_boat_id, destruction_time))
        
        # 从所有船的目标列表中移除该目标
        for boat_id in list(self.boat_targets.keys()):
            boat_target_list = self.boat_targets[boat_id]
            if isinstance(boat_target_list, list) and target_id in boat_target_list:
                boat_target_list.remove(target_id)
                print(f"[FSM] DEFENSIVE: 从船只 {boat_id} 的目标列表中移除已击沉目标 {target_id}")
                
                # 如果这是负责击沉目标的船只，通知其他可能分配了该目标的船只
                if boat_id == responsible_boat_id:
                    self._notify_other_boats_target_destroyed(target_id, boat_id)
        
        # 从检测目标中移除
        if target_id in self.detected_targets:
            del self.detected_targets[target_id]
            
        print(f"[FSM] DEFENSIVE: 目标 {target_id} 被击沉，负责船只: {responsible_boat_id}")

    def _notify_other_boats_target_destroyed(self, target_id: str, responsible_boat_id: int):
        """通知其他船只目标已被击沉"""
        for boat_id in self.boat_targets:
            if boat_id != responsible_boat_id:
                boat_target_list = self.boat_targets[boat_id]
                if isinstance(boat_target_list, list) and target_id in boat_target_list:
                    boat_target_list.remove(target_id)
                    print(f"[FSM] DEFENSIVE: 通知船只 {boat_id} 移除已被其他船只击沉的目标 {target_id}")

    def _get_next_target_for_boat(self, boat: WhiteBoat) -> Optional[str]:
        """为无人船获取下一个目标"""
        boat_target_list = self.boat_targets.get(boat.id, [])
        
        if isinstance(boat_target_list, list) and boat_target_list:
            # 返回列表中的第一个目标
            next_target_id = boat_target_list[0]
            print(f"[FSM] DEFENSIVE: 船只 {boat.id} 转向下一个目标 {next_target_id}")
            return next_target_id
        else:
            # 目标列表为空，在原地警戒
            print(f"[FSM] DEFENSIVE: 船只 {boat.id} 目标列表已清空，保持警戒")
            return None


def create_fsm_defense_agent() -> FSMDefenseAgent:
    """工厂函数，创建FSM防御策略智能体"""
    return FSMDefenseAgent()
